From 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 16 Feb 2013 13:40:09 -0800 Subject: Checkpoint: implement new state machine, compiling, WIP --- apps/commander/commander.c | 410 +++++++++--------- apps/commander/state_machine_helper.c | 472 +++++++++++++-------- apps/commander/state_machine_helper.h | 157 +------ apps/controllib/fixedwing.cpp | 33 +- apps/drivers/blinkm/blinkm.cpp | 37 +- .../fixedwing_att_control_main.c | 163 +++---- apps/mavlink/mavlink.c | 110 +++-- apps/mavlink/orb_listener.c | 2 +- apps/mavlink_onboard/mavlink.c | 110 ++--- .../multirotor_att_control_main.c | 84 ++-- apps/sensors/sensors.cpp | 108 ++--- apps/uORB/topics/manual_control_setpoint.h | 12 +- apps/uORB/topics/rc_channels.h | 22 +- apps/uORB/topics/vehicle_status.h | 94 ++-- 14 files changed, 899 insertions(+), 915 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b..8716caef7 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -124,18 +124,24 @@ extern struct system_load_s system_load; static int leds; static int buzzer; static int mavlink_fd; + +/* flags */ static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* Main state machine */ +static struct vehicle_status_s current_status; static orb_advert_t stat_pub; +/* XXX ? */ // static uint16_t nofix_counter = 0; // static uint16_t gotfix_counter = 0; +/* XXX ? */ static unsigned int failsafe_lowlevel_timeout_ms; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ /* pthread loops */ static void *orb_receive_loop(void *arg); @@ -252,18 +258,19 @@ enum AUDIO_PATTERN { int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) { +#warning add alarms for state transitions /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } +// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || +// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); +// } /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } +// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); +// } /* Trigger Tetris on being bored */ @@ -277,6 +284,7 @@ void tune_confirm(void) void tune_error(void) { + /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -795,19 +803,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX implement this - } else { +// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { result = VEHICLE_CMD_RESULT_DENIED; - } +// } } break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -815,9 +825,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* request to disarm */ - + // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -830,7 +840,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -871,16 +881,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -893,16 +908,20 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting mag cal"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -916,12 +935,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + // XXX implement this mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); tune_confirm(); + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + } else { mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); result = VEHICLE_CMD_RESULT_DENIED; @@ -933,15 +957,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting trim cal"); tune_confirm(); do_rc_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished trim cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -954,16 +981,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "CMD starting accel cal"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1262,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); /* welcome user */ - warnx("I am in command now!\n"); + warnx("[commander] starting"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1270,33 +1300,39 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds"); } if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -1315,6 +1351,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } + // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ @@ -1325,7 +1362,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ uint16_t counter = 0; - uint8_t flight_env; /* Initialize to 0.0V */ float battery_voltage = 0.0f; @@ -1350,11 +1386,13 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); uint64_t last_global_position_time = 0; + /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); @@ -1365,10 +1403,13 @@ int commander_thread_main(int argc, char *argv[]) * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. */ + + /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1502,45 +1543,36 @@ int commander_thread_main(int argc, char *argv[]) /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); - - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_BLUE); - } - - /* toggle error led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_AMBER); +// if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || +// current_status.state_machine == SYSTEM_STATE_AUTO || +// current_status.state_machine == SYSTEM_STATE_MANUAL)) { +// /* armed */ +// led_toggle(LED_BLUE); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); + /* toggle error led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_AMBER); - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); - // } else { - // led_off(LED_AMBER); - // } - } + } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - last_idle_time = system_load.tasks[0].total_runtime; - } + last_idle_time = system_load.tasks[0].total_runtime; } // // XXX Export patterns and threshold to parameters @@ -1583,13 +1615,14 @@ int commander_thread_main(int argc, char *argv[]) low_voltage_counter++; } - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + /* Critical, this is rather an emergency, change state machine */ else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // XXX implement this +// state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1698,7 +1731,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { @@ -1734,85 +1767,112 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); + + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { + + /* assuming a fixed wing, fall back to direct pass-through */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, fall back to m */ + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { + + /* assuming a fixed wing, set to direct pass-through as requested */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.flag_control_position_enabled = true; + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + + } else { + /* center stick position, set seatbelt/simple control */ + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if manual stability control modes have to be switched + * Check if land/RTL is requested */ - if (!isfinite(sp_man.manual_sas_switch)) { + if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + current_status.flag_land_requested = false; - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + current_status.flag_land_requested = true; } else { /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.flag_mission_activated = false; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.flag_mission_activated = true; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.flag_mission_activated = false; + + } else { + + /* center switch position, set default */ + current_status.flag_mission_activated = false; // XXX default? + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } /* @@ -1826,7 +1886,7 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = false; stick_on_counter = 0; } else { @@ -1838,7 +1898,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = true; stick_on_counter = 0; } else { @@ -1847,37 +1907,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; @@ -1965,12 +1994,13 @@ int commander_thread_main(int argc, char *argv[]) /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); +#warning fix this +// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } } else { @@ -2006,18 +2036,20 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); + // XXX what is this? /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } +// if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && +// current_status.flag_preflight_gyro_calibration == false && +// current_status.flag_preflight_mag_calibration == false && +// current_status.flag_preflight_accel_calibration == false) { +// /* All ok, no calibration going on, go to standby */ +// do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); +// } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); +#warning fix this +// publish_armed_status(¤t_status); orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2043,7 +2075,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - warnx("exiting..\n"); + warnx("exiting"); fflush(stdout); thread_running = false; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index bea388a10..e1ec73110 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,182 +40,297 @@ #include #include +#include #include #include #include #include +#include #include #include #include "state_machine_helper.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; /** - * Transition from one state to another + * Transition from one navigation state to another */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { - int invalid_state = false; + bool valid_transition = false; int ret = ERROR; - commander_state_machine_t old_state = current_status->state_machine; + if (current_status->navigation_state == new_state) { + warnx("Navigation state not changed"); - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + } else { - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } + switch (new_state) { + case NAVIGATION_STATE_INIT: + + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_STANDBY: + + if (current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MANUAL: + + if ( + ( current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) + && current_status->arming_state == ARMING_STATE_ARMED) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LOITER: + + if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) + && current_status->flag_global_position_valid) + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + if ( + (current_status->navigation_state == NAVIGATION_STATE_STANDBY + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + || current_status->navigation_state == NAVIGATION_STATE_LAND) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MISSION: + + if ( + current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_RTL: + + if ( + current_status->navigation_state == NAVIGATION_STATE_MISSION + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_TAKEOFF: + + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LAND: + if (current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_REBOOT: + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->flag_hil_enabled) { + valid_transition = true; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + default: + warnx("Unknown navigation state"); + break; } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; + } - case SYSTEM_STATE_GROUND_ERROR: + if (valid_transition) { + current_status->navigation_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); +// publish_armed_status(current_status); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); + } - /* set system flags according to state */ + return ret; +} - /* prevent actuators from arming */ - current_status->flag_system_armed = false; +/** + * Transition from one arming state to another + */ +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; + if (current_status->arming_state == new_state) { + warnx("Arming state not changed"); - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + } else { - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } + switch (new_state) { - break; + case ARMING_STATE_INIT: - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + if (current_status->arming_state == ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + valid_transition = true; + } + break; - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } + case ARMING_STATE_STANDBY: - break; + // TODO check for sensors + // XXX check if coming from reboot? + if (current_status->arming_state == ARMING_STATE_INIT) { - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); + valid_transition = true; + } + break; - /* standby enforces disarmed */ - current_status->flag_system_armed = false; + case ARMING_STATE_ARMED: - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - case SYSTEM_STATE_GROUND_READY: + mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + valid_transition = true; + } + break; - /* set system flags according to state */ + case ARMING_STATE_MISSION_ABORT: - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; + if (current_status->arming_state == ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + valid_transition = true; + } + break; - case SYSTEM_STATE_AUTO: + case ARMING_STATE_ERROR: - /* set system flags according to state */ + if (current_status->arming_state == ARMING_STATE_ARMED + || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_INIT) { - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + valid_transition = true; + } + break; - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; + case ARMING_STATE_REBOOT: - case SYSTEM_STATE_STABILIZED: + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_ERROR) { - /* set system flags according to state */ - current_status->flag_system_armed = true; + mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); + valid_transition = true; + // XXX reboot here? + } + break; - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; + case ARMING_STATE_IN_AIR_RESTORE: - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; + if (current_status->arming_state == ARMING_STATE_INIT) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + valid_transition = true; + } + break; + default: + warnx("Unknown arming state"); + break; + } } - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; + if (valid_transition) { + current_status->arming_state = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); +// publish_armed_status(current_status); ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); } return ret; } + + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { /* publish the new state */ @@ -223,70 +338,69 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->timestamp = hrt_absolute_time(); /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } + +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} +//void publish_armed_status(const struct vehicle_status_s *current_status) +//{ +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +//} /* * Private functions, update the state machine */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} +//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// warnx("EMERGENCY HANDLER\n"); +// /* Depending on the current state go to one of the error states */ +// +// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); +// +// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { +// +// // DO NOT abort mission +// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); +// +// } else { +// warnx("Unknown system state: #%d\n", current_status->state_machine); +// } +//} + +//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +//{ +// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself +// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); +// +// } else { +// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); +// } +// +//} @@ -488,6 +602,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +#if 0 void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { @@ -750,3 +865,4 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ return ret; } +#endif diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 2f2ccc729..bf9296caf 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,163 +47,10 @@ #include #include -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); - -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - - #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index d9637b4a7..9a6919535 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" + namespace control { @@ -294,7 +295,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.navigation_state == NAVIGATION_STATE_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,8 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.navigation_state == NAVIGATION_STATE_MISSION || + _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); @@ -340,25 +341,25 @@ void BlockMultiModeBacksideAutopilot::update() } - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { +// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +#warning fix the different manual modes _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = - _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } +// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, +// _att.rollspeed, _att.pitchspeed, _att.yawspeed); +// +// _actuators.control[CH_AIL] = _stabilization.getAileron(); +// _actuators.control[CH_ELV] = - _stabilization.getElevator(); +// _actuators.control[CH_RDR] = _stabilization.getRudder(); +// _actuators.control[CH_THR] = _manual.throttle; +// } } // update all publications diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c..6aff27e4c 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -554,26 +554,27 @@ BlinkM::led() led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - led_color_4 = LED_GREEN; - break; - } +#warning fix this +// switch((int)vehicle_status_raw.flight_mode) { +// case VEHICLE_FLIGHT_MODE_MANUAL: +// led_color_4 = LED_AMBER; +// break; +// +// case VEHICLE_FLIGHT_MODE_STAB: +// led_color_4 = LED_YELLOW; +// break; +// +// case VEHICLE_FLIGHT_MODE_HOLD: +// led_color_4 = LED_BLUE; +// break; +// +// case VEHICLE_FLIGHT_MODE_AUTO: +// led_color_4 = LED_GREEN; +// break; +// } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat´s */ + /* handling used sat�s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index aa9db6d52..12f06cdd2 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -185,87 +185,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } +#warning fix this +// if (vstatus.state_machine == SYSTEM_STATE_AUTO || +// vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* set flaps to zero */ +// actuators.control[4] = 0.0f; +// +// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { +// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +// if (vstatus.rc_signal_lost) { +// +// /* put plane into loiter */ +// att_sp.roll_body = 0.3f; +// att_sp.pitch_body = 0.0f; +// +// /* limit throttle to 60 % of last value if sane */ +// if (isfinite(manual_sp.throttle) && +// (manual_sp.throttle >= 0.0f) && +// (manual_sp.throttle <= 1.0f)) { +// att_sp.thrust = 0.6f * manual_sp.throttle; +// +// } else { +// att_sp.thrust = 0.0f; +// } +// +// att_sp.yaw_body = 0; +// +// // XXX disable yaw control, loiter +// +// } else { +// +// att_sp.roll_body = manual_sp.roll; +// att_sp.pitch_body = manual_sp.pitch; +// att_sp.yaw_body = 0; +// att_sp.thrust = manual_sp.throttle; +// } +// +// att_sp.timestamp = hrt_absolute_time(); +// +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* pass through flaps */ +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// +// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// /* directly pass through values */ +// actuators.control[0] = manual_sp.roll; +// /* positive pitch means negative actuator -> pull up */ +// actuators.control[1] = manual_sp.pitch; +// actuators.control[2] = manual_sp.yaw; +// actuators.control[3] = manual_sp.throttle; +// +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// } +// } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b958d5f96..4636ee36e 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -195,95 +195,85 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* reset MAVLink mode bitfield */ *mavlink_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { + if (v_status.navigation_state == NAVIGATION_STATE_MISSION + || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_RTL + || v_status.navigation_state == NAVIGATION_STATE_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (armed.armed) { + if (v_status.flag_system_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; + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - } else { - *mavlink_state = MAV_STATE_UNINIT; - } + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } - break; + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; + *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; + } - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; + *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; + } - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - break; + /** + * Set mavlink state + **/ - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + /* set calibration state */ + if (v_status.flag_preflight_gyro_calibration || + v_status.flag_preflight_mag_calibration || + v_status.flag_preflight_accel_calibration) { - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + *mavlink_state = MAV_STATE_CALIBRATING; - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + } else if (v_status.flag_system_emergency) { - case SYSTEM_STATE_GROUND_ERROR: *mavlink_state = MAV_STATE_EMERGENCY; - break; - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_LOITER + || v_status.navigation_state == NAVIGATION_STATE_MISSION + || v_status.navigation_state == NAVIGATION_STATE_RTL + || v_status.navigation_state == NAVIGATION_STATE_LAND + || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + + *mavlink_state = MAV_STATE_ACTIVE; + + } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { + + *mavlink_state = MAV_STATE_STANDBY; + + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + + *mavlink_state = MAV_STATE_UNINIT; + } else { + + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } } @@ -688,7 +678,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&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); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ set_hil_on_off(v_status.flag_hil_enabled); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 9f85d5801..15066010c 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -275,7 +275,7 @@ l_vehicle_status(struct listener *l) mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, - v_status.state_machine, + v_status.navigation_state, mavlink_state); } diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600..6babe14ae 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -296,60 +296,60 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *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; - } +// 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; +// } } @@ -434,7 +434,7 @@ int mavlink_thread_main(int argc, char *argv[]) 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); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c..da7550f79 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[]) } /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } - } - } +#warning fix this +// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// +// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// +// } else { +// /* +// * This mode SHOULD be the default mode, which is: +// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS +// * +// * However, we fall back to this setting for all other (nonsense) +// * settings as well. +// */ +// +// /* only move setpoint if manual input is != 0 */ +// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// first_time_after_yaw_speed_control = true; +// +// } else { +// if (first_time_after_yaw_speed_control) { +// att_sp.yaw_body = att.yaw; +// first_time_after_yaw_speed_control = false; +// } +// +// control_yaw_position = true; +// } +// } +// } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); @@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[]) } } else { +#warning fix this /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; - - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } +// if (state.flag_control_rates_enabled && +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { +// rates_sp.roll = manual.roll; +// +// rates_sp.pitch = manual.pitch; +// rates_sp.yaw = manual.yaw; +// rates_sp.thrust = manual.throttle; +// rates_sp.timestamp = hrt_absolute_time(); +// } } } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9..28579bc70 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -195,13 +195,11 @@ private: int rc_map_yaw; int rc_map_throttle; - int rc_map_manual_override_sw; - int rc_map_auto_mode_sw; + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_mission_sw; - int rc_map_manual_mode_sw; - int rc_map_sas_mode_sw; - int rc_map_rtl_sw; - int rc_map_offboard_ctrl_mode_sw; +// int rc_map_offboard_ctrl_mode_sw; int rc_map_flaps; @@ -241,13 +239,11 @@ private: param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_manual_override_sw; - param_t rc_map_auto_mode_sw; + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_mission_sw; - param_t rc_map_manual_mode_sw; - param_t rc_map_sas_mode_sw; - param_t rc_map_rtl_sw; - param_t rc_map_offboard_ctrl_mode_sw; +// param_t rc_map_offboard_ctrl_mode_sw; param_t rc_map_flaps; @@ -436,16 +432,15 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); - _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); - _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); - _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); - _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + +// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -579,33 +574,25 @@ Sensors::parameters_update() warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { - warnx("Failed getting override sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { - warnx("Failed getting auto mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); } - if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { - warnx("Failed getting manual mode sw chan index"); + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("Failed getting return sw chan index"); } - if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { - warnx("Failed getting rtl sw chan index"); + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + warnx("Failed getting mission sw chan index"); } - if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { - warnx("Failed getting sas mode sw chan index"); + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); } - if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { - warnx("Failed getting offboard control mode sw chan index"); - } +// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { +// warnx("Failed getting offboard control mode sw chan index"); +// } if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { warnx("Failed getting mode aux 1 index"); @@ -649,15 +636,13 @@ Sensors::parameters_update() _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; - _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; - _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; - _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; +// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; @@ -1122,10 +1107,10 @@ Sensors::ppm_poll() manual_control.yaw = NAN; manual_control.throttle = NAN; - manual_control.manual_mode_switch = NAN; - manual_control.manual_sas_switch = NAN; - manual_control.return_to_launch_switch = NAN; - manual_control.auto_offboard_input_switch = NAN; + manual_control.mode_switch = NAN; + manual_control.return_switch = NAN; + manual_control.mission_switch = NAN; +// manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1211,11 +1196,14 @@ Sensors::ppm_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* override switch input */ - manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); - /* mode switch input */ - manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + + /* land switch input */ + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + + /* land switch input */ + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1227,21 +1215,17 @@ Sensors::ppm_poll() } } - if (_rc.function[MANUAL_MODE] >= 0) { - manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); - } - - if (_rc.function[SAS_MODE] >= 0) { - manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } - if (_rc.function[RTL] >= 0) { - manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } +// if (_rc.function[OFFBOARD_MODE] >= 0) { +// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); +// } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 261a8a4ad..cfee81ea2 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -56,17 +56,17 @@ struct manual_control_setpoint_s { float yaw; /**< rudder / yaw rate / yaw */ float throttle; /**< throttle / collective thrust / altitude */ - float manual_override_switch; /**< manual override mode (mandatory) */ - float auto_mode_switch; /**< auto mode switch (mandatory) */ + float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ - float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ - float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ - float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + // XXX needed or parameter? + //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ float flaps; /**< flap position */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 9dd54df91..a0bb25af4 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -68,18 +68,16 @@ enum RC_CHANNELS_FUNCTION ROLL = 1, PITCH = 2, YAW = 3, - OVERRIDE = 4, - AUTO_MODE = 5, - MANUAL_MODE = 6, - SAS_MODE = 7, - RTL = 8, - OFFBOARD_MODE = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, + MODE = 4, + RETURN = 5, + MISSION = 6, + OFFBOARD_MODE = 7, + FLAPS = 8, + AUX_1 = 9, + AUX_2 = 10, + AUX_3 = 11, + AUX_4 = 12, + AUX_5 = 13, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5..f9c4a5fff 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -59,21 +59,30 @@ */ /* State Machine */ -typedef enum -{ - SYSTEM_STATE_PREFLIGHT = 0, - SYSTEM_STATE_STANDBY = 1, - SYSTEM_STATE_GROUND_READY = 2, - SYSTEM_STATE_MANUAL = 3, - SYSTEM_STATE_STABILIZED = 4, - SYSTEM_STATE_AUTO = 5, - SYSTEM_STATE_MISSION_ABORT = 6, - SYSTEM_STATE_EMCY_LANDING = 7, - SYSTEM_STATE_EMCY_CUTOFF = 8, - SYSTEM_STATE_GROUND_ERROR = 9, - SYSTEM_STATE_REBOOT= 10, - -} commander_state_machine_t; +typedef enum { + NAVIGATION_STATE_INIT = 0, + NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_TAKEOFF, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_GROUND_ERROR, + NAVIGATION_STATE_REBOOT +} navigation_state_t; + +typedef enum { + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -86,25 +95,25 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ -enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ - VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -}; - -enum VEHICLE_MANUAL_CONTROL_MODE { - VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ - VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -}; - -enum VEHICLE_MANUAL_SAS_MODE { - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ - VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ - VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -}; +//enum VEHICLE_FLIGHT_MODE { +// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ +// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ +// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ +// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ +//}; +// +//enum VEHICLE_MANUAL_CONTROL_MODE { +// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ +// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ +// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +//}; +// +//enum VEHICLE_MANUAL_SAS_MODE { +// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ +// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ +// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ +// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ +//}; /** * Should match 1:1 MAVLink's MAV_TYPE ENUM @@ -134,7 +143,7 @@ enum VEHICLE_TYPE { enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ }; @@ -150,17 +159,17 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ - //uint64_t failsave_highlevel_begin; TO BE COMPLETED +// uint64_t failsave_highlevel_begin; TO BE COMPLETED + + navigation_state_t navigation_state; /**< current navigation state */ + arming_state_t arming_state; /**< current arming state */ - commander_state_machine_t state_machine; /**< current flight state, main state machine */ - enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ /* system flags - these represent the state predicates */ bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_system_emergency; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ @@ -170,6 +179,9 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_land_requested; /**< true if land requested */ + bool flag_mission_activated; /**< true if in mission mode */ + bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; @@ -185,7 +197,7 @@ struct vehicle_status_s uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - //bool failsave_highlevel; + bool failsave_highlevel; /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; -- cgit v1.2.3 From 3bc385c789f2b39cda066551ff1d5b767ab26aec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Feb 2013 15:04:01 -0800 Subject: Checkpoint: Added arming state check --- apps/commander/commander.c | 6 +++++- apps/commander/state_machine_helper.c | 22 ++++++++++++++++++++++ apps/commander/state_machine_helper.h | 2 ++ apps/sensors/sensor_params.c | 10 ++++------ apps/sensors/sensors.cpp | 2 +- apps/uORB/topics/vehicle_status.h | 2 ++ 6 files changed, 36 insertions(+), 8 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8716caef7..1bfdd5660 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -2054,9 +2054,13 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } + /* make changes in state machine if needed */ + update_state_machine(stat_pub, ¤t_status, mavlink_fd); + + + /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; - fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1ec73110..d89be781a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,6 +53,28 @@ #include "state_machine_helper.h" +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* check arming first */ + if (current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); + } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (current_status->flag_system_sensors_ok) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } + + /* now determine the navigation state */ +} + /** * Transition from one navigation state to another */ diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf..ed18bfbd2 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,6 +47,8 @@ #include #include +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbba..df7f66116 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -158,13 +158,11 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 28579bc70..ce404ee7e 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -438,7 +438,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f9c4a5fff..ba9b9793b 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -170,6 +170,8 @@ struct vehicle_status_s bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; + bool flag_system_sensors_ok; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ -- cgit v1.2.3 From 47b05eeb87191fd0b380de008299f85262bc8953 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Feb 2013 23:07:07 -0800 Subject: Checkpoint, arming/disarming still has a bug --- apps/commander/commander.c | 225 ++++++++++++++-------------- apps/commander/state_machine_helper.c | 272 +++++++++++++++++++--------------- apps/commander/state_machine_helper.h | 2 - apps/mavlink/mavlink.c | 15 +- apps/uORB/topics/vehicle_status.h | 34 ++++- 5 files changed, 304 insertions(+), 244 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1bfdd5660..4e470f1d9 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -817,22 +817,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } /* request to disarm */ // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } } } break; @@ -840,7 +842,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -883,25 +885,26 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param1) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "starting gyro cal"); +// tune_confirm(); +// do_gyro_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished gyro cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -910,24 +913,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param2) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting mag cal"); +// tune_confirm(); +// do_mag_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished mag cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -935,21 +939,22 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - // XXX implement this - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// // XXX implement this +// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -957,49 +962,51 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting trim cal"); +// tune_confirm(); +// do_rc_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished trim cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); +// tune_confirm(); +// do_accel_calibration(status_pub, ¤t_status); +// tune_confirm(); +// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -1315,7 +1322,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; @@ -1336,6 +1343,9 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + // XXX for now just set sensors as initialized + current_status.flag_system_sensors_initialized = true; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1886,8 +1896,8 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = false; - stick_on_counter = 0; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + stick_off_counter = 0; } else { stick_off_counter++; @@ -1898,7 +1908,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = true; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { @@ -2054,9 +2064,6 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } - /* make changes in state machine if needed */ - update_state_machine(stat_pub, ¤t_status, mavlink_fd); - /* Store old modes to detect and act on state transitions */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index d89be781a..a0b786aab 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,33 +53,12 @@ #include "state_machine_helper.h" -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* check arming first */ - if (current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); - } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (current_status->flag_system_sensors_ok) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } - - /* now determine the navigation state */ -} - /** * Transition from one navigation state to another */ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { + bool valid_path = false; bool valid_transition = false; int ret = ERROR; @@ -89,41 +68,43 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else { switch (new_state) { - case NAVIGATION_STATE_INIT: - - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); - valid_transition = true; - } - break; - case NAVIGATION_STATE_STANDBY: - if (current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->navigation_state == NAVIGATION_STATE_MANUAL + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); + } + valid_path = true; } break; case NAVIGATION_STATE_MANUAL: - if ( - ( current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) - && current_status->arming_state == ARMING_STATE_ARMED) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + + /* only check for armed flag when coming from STANDBY XXX does that make sense? */ + if (current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); valid_transition = true; + valid_path = true; } break; @@ -133,67 +114,132 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ || current_status->navigation_state == NAVIGATION_STATE_LOITER || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); valid_transition = true; + valid_path = true; } break; case NAVIGATION_STATE_LOITER: - if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) - && current_status->flag_global_position_valid) - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + /* Check for position lock when coming from MANUAL or SEATBELT */ + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + + } else { + mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); valid_transition = true; - } + valid_path = true; + } break; case NAVIGATION_STATE_AUTO_READY: - if ( - (current_status->navigation_state == NAVIGATION_STATE_STANDBY - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - || current_status->navigation_state == NAVIGATION_STATE_LAND) { + /* coming from STANDBY pos and home lock are needed */ + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); - valid_transition = true; + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + } + valid_path = true; + /* coming from LAND home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { + + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_MISSION: - if ( - current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + /* coming from TAKEOFF or RTL is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_RTL: - if ( - current_status->navigation_state == NAVIGATION_STATE_MISSION - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + /* coming from MISSION is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } + valid_path = true; } break; @@ -201,8 +247,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + /* TAKEOFF is straight forward from AUTO READY */ + mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); valid_transition = true; + valid_path = true; } break; @@ -210,27 +258,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_RTL || current_status->navigation_state == NAVIGATION_STATE_LOITER) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); - valid_transition = true; - } - break; - - case NAVIGATION_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->flag_hil_enabled) { + mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + valid_path = true; } - break; default: @@ -244,7 +275,9 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ state_machine_publish(status_pub, current_status, mavlink_fd); // publish_armed_status(current_status); ret = OK; - } else { + } + + if (!valid_path){ mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); } @@ -269,7 +302,8 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_INIT: if (current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + + mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); valid_transition = true; } break; @@ -280,8 +314,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat // XXX check if coming from reboot? if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); - valid_transition = true; + if (current_status->flag_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); + } } break; @@ -290,16 +328,16 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } break; - case ARMING_STATE_MISSION_ABORT: + case ARMING_STATE_ABORT: if (current_status->arming_state == ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); valid_transition = true; } break; @@ -307,10 +345,10 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_ERROR: if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_ABORT || current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); valid_transition = true; } break; @@ -320,16 +358,18 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_ERROR) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); valid_transition = true; - // XXX reboot here? + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } break; case ARMING_STATE_IN_AIR_RESTORE: if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); valid_transition = true; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index ed18bfbd2..bf9296caf 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,8 +47,6 @@ #include #include -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 4636ee36e..e25f1be27 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -248,26 +248,19 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_LOITER - || v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + } else if (v_status.arming_state == ARMING_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index ba9b9793b..874cf256c 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,8 +60,7 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, NAVIGATION_STATE_LOITER, @@ -70,15 +69,13 @@ typedef enum { NAVIGATION_STATE_RTL, NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, - NAVIGATION_STATE_GROUND_ERROR, - NAVIGATION_STATE_REBOOT } navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, ARMING_STATE_ARMED, - ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ABORT, ARMING_STATE_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE @@ -95,6 +92,22 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_ASSISTED, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; + //enum VEHICLE_FLIGHT_MODE { // VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ // VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ @@ -168,9 +181,18 @@ struct vehicle_status_s /* system flags - these represent the state predicates */ + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; - bool flag_system_sensors_ok; + bool flag_system_in_air_restore; /**< true if we can restore in mid air */ + bool flag_system_sensors_initialized; + bool flag_system_arming_requested; + bool flag_system_disarming_requested; + bool flag_system_reboot_requested; + bool flag_system_on_ground; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ -- cgit v1.2.3 From 5eac78d7645becc486bc6a43852b9631e62465b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Feb 2013 13:52:18 -0800 Subject: Checkpoint: Added DESCENT state --- apps/commander/state_machine_helper.c | 51 ++++++++++++++++++++++------------- apps/uORB/topics/vehicle_status.h | 3 ++- 2 files changed, 34 insertions(+), 20 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a0b786aab..68b4bbe30 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -119,6 +119,16 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ valid_path = true; } break; + case NAVIGATION_STATE_DESCENT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); + valid_transition = true; + valid_path = true; + } + break; case NAVIGATION_STATE_LOITER: @@ -146,25 +156,28 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ /* coming from STANDBY pos and home lock are needed */ if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); + } else if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; } valid_path = true; /* coming from LAND home lock is needed */ } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - if (current_status->flag_valid_launch_position) { + if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); } valid_path = true; } @@ -183,15 +196,15 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } valid_path = true; @@ -219,15 +232,13 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; } valid_path = true; @@ -256,7 +267,8 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ case NAVIGATION_STATE_LAND: if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; @@ -315,6 +327,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_INIT) { if (current_status->flag_system_sensors_initialized) { + current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); valid_transition = true; } else { @@ -327,7 +340,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - + current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 874cf256c..dde978cae 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -63,12 +63,13 @@ typedef enum { NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_DESCENT, NAVIGATION_STATE_LOITER, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_MISSION, NAVIGATION_STATE_RTL, - NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TAKEOFF } navigation_state_t; typedef enum { -- cgit v1.2.3 From b7faaca435551064e6fdb070a9e762b4146ae4e8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Feb 2013 16:35:34 -0800 Subject: Checkpoint: Arming/Disarming works --- apps/commander/commander.c | 18 ++++++++++++------ apps/commander/state_machine_helper.c | 9 +++++++++ apps/mavlink_onboard/mavlink.c | 2 +- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27c5f1989..ac535dd9a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1649,6 +1649,13 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + } else { + // XXX: Add emergency stuff if sensors are lost + } + /* * Check for valid position information. @@ -1894,12 +1901,11 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { +// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || +// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || +// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) +// ) && + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 68b4bbe30..f1de99e4d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -304,8 +304,11 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat bool valid_transition = false; int ret = ERROR; + warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); + if (current_status->arming_state == new_state) { warnx("Arming state not changed"); + valid_transition = true; } else { @@ -333,6 +336,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat } else { mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); } + + } else if (current_status->arming_state == ARMING_STATE_ARMED) { + + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); + valid_transition = true; } break; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 6babe14ae..c5a1e82a8 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (armed->armed) { + if (v_status->flag_system_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; -- cgit v1.2.3 From aab6214cdcc630dce1f64ba9220bc1f5b10b6af1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Feb 2013 12:32:47 -0800 Subject: Checkpoint: Added HIL state, arming/disarming works now, also from GQC --- apps/commander/commander.c | 498 +++++++++++++++++----------------- apps/commander/state_machine_helper.c | 199 +++++++++----- apps/uORB/topics/vehicle_status.h | 6 + 3 files changed, 382 insertions(+), 321 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ac535dd9a..6b026f287 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -797,302 +797,297 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* announce command handling */ tune_confirm(); - - /* supported command handling start */ - /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - // XXX implement this - -// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { - result = VEHICLE_CMD_RESULT_DENIED; -// } - } - break; + case VEHICLE_CMD_DO_SET_MODE: - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - /* request to disarm */ - // XXX this arms it instad of disarming - } else if ((int)cmd->param1 == 0) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - } - } - break; + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; } } - } - break; -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* transition to init state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// mavlink_log_info(mavlink_fd, "starting gyro cal"); -// tune_confirm(); -// do_gyro_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished gyro cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* transition to init state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// mavlink_log_info(mavlink_fd, "starting mag cal"); -// tune_confirm(); -// do_mag_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished mag cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } + break; - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// // XXX implement this -// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } + case VEHICLE_CMD_COMPONENT_ARM_DISARM: - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// mavlink_log_info(mavlink_fd, "starting trim cal"); -// tune_confirm(); -// do_rc_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished trim cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); -// tune_confirm(); -// do_accel_calibration(status_pub, ¤t_status); -// tune_confirm(); -// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } } - } - break; + break; - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { - } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ + mavlink_log_info(mavlink_fd, "starting gyro cal"); + tune_confirm(); + do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished gyro cal"); + tune_confirm(); - if (((int)(cmd->param1)) == 0) { + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + mavlink_log_info(mavlink_fd, "starting mag cal"); + tune_confirm(); + do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_FAILED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; } - } else if (((int)(cmd->param1)) == 1) { + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + // XXX add this again + // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + // + // // XXX implement this + // mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + // tune_confirm(); + // + // /* back to standby state */ + // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + // + // } else { + // mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + // XXX add this again + // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + // mavlink_log_info(mavlink_fd, "starting trim cal"); + // tune_confirm(); + // do_rc_calibration(status_pub, ¤t_status); + // mavlink_log_info(mavlink_fd, "finished trim cal"); + // tune_confirm(); + // + // /* back to standby state */ + // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + // + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // + // } else { + // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); result = VEHICLE_CMD_RESULT_ACCEPTED; + mavlink_log_info(mavlink_fd, "starting acc cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished acc cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; } - result = VEHICLE_CMD_RESULT_FAILED; + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } - } - break; + break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); @@ -1326,6 +1321,7 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; /* neither manual nor offboard control commands have been received */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index f1de99e4d..aae119d35 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -413,6 +413,63 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat return ret; } +/** + * Transition from one hil state to another + */ +int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) @@ -684,7 +741,78 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_system_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} #if 0 @@ -821,76 +949,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur } -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - return ret; -} uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f5ab91bad..27a471f13 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -82,6 +82,11 @@ typedef enum { ARMING_STATE_IN_AIR_RESTORE } arming_state_t; +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -177,6 +182,7 @@ struct vehicle_status_s navigation_state_t navigation_state; /**< current navigation state */ arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ -- cgit v1.2.3 From 0e29f2505a599d473244b0bb7e4b309d392ebb3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Feb 2013 10:38:06 -0800 Subject: Checkpoint: New hierarchical states --- Documentation/flight_mode_state_machine.odg | Bin 18105 -> 24059 bytes apps/commander/commander.c | 131 +++--- apps/commander/state_machine_helper.c | 614 ++++++++++++---------------- apps/commander/state_machine_helper.h | 4 +- apps/uORB/topics/vehicle_status.h | 89 ++-- 5 files changed, 367 insertions(+), 471 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index b630ecb40..a50366bcb 100644 Binary files a/Documentation/flight_mode_state_machine.odg and b/Documentation/flight_mode_state_machine.odg differ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b026f287..8b9e7c49c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); - - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - - /* assuming a fixed wing, fall back to direct pass-through */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, fall back to m */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - - /* assuming a fixed wing, set to direct pass-through as requested */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.flag_control_position_enabled = true; - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - - } else { - /* center stick position, set seatbelt/simple control */ - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); + + /* no valid stick position, go to default */ + + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + + } else { + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if land/RTL is requested - */ + * Check if land/RTL is requested + */ if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.flag_land_requested = false; + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.flag_land_requested = true; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { /* top switch position */ - current_status.flag_mission_activated = true; + current_status.mission_switch = MISSION_SWITCH_MISSION; } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { /* bottom switch position */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else { /* center switch position, set default */ - current_status.flag_mission_activated = false; // XXX default? + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + if (current_status.arming_state == ARMING_STATE_ARMED) { + + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); + } + } } /* handle the case where RC signal was regained */ @@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ -// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || -// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || -// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) -// ) && + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); @@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aae119d35..2e4935471 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -54,422 +54,330 @@ /** - * Transition from one navigation state to another + * Transition from one sytem state to another */ -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) +void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_path = false; - bool valid_transition = false; int ret = ERROR; + system_state_t new_system_state; - if (current_status->navigation_state == new_state) { - warnx("Navigation state not changed"); - } else { - - switch (new_state) { - case NAVIGATION_STATE_STANDBY: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_MANUAL: - - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - /* only check for armed flag when coming from STANDBY XXX does that make sense? */ - if (current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_SEATBELT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_DESCENT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LOITER: - - /* Check for position lock when coming from MANUAL or SEATBELT */ - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - - } else { - mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* coming from STANDBY pos and home lock are needed */ - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); + /* + * Firstly evaluate mode switch position + * */ - } else if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_system_state = SYSTEM_STATE_MANUAL; - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - /* coming from LAND home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - - if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_MISSION: - - /* coming from TAKEOFF or RTL is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_RTL: - - /* coming from MISSION is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } - valid_path = true; - } - break; + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL); + } - case NAVIGATION_STATE_TAKEOFF: + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT); + } else { + mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); + + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL; + } + } + } - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + /* + * Next up, check for + */ - /* TAKEOFF is straight forward from AUTO READY */ - mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_LAND: - if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to LAND state"); - valid_transition = true; - valid_path = true; - } - break; + /* Update the system state in case it has changed */ + if (current_status->system_state != new_system_state) { - default: - warnx("Unknown navigation state"); - break; + /* Check if the transition is valid */ + if (system_state_update(current_status->system_state, new_system_state) == OK) { + current_status->system_state = new_system_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + } else { + mavlink_log_critical(mavlink_fd, "System state transition not valid"); } } - if (valid_transition) { - current_status->navigation_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } - - if (!valid_path){ - mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); - } - - return ret; + return; } -/** - * Transition from one arming state to another +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. */ -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); +int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { - if (current_status->arming_state == new_state) { - warnx("Arming state not changed"); - valid_transition = true; - - } else { + int ret = ERROR; - switch (new_state) { + /* only check transition if the new state is actually different from the current one */ + if (new_system_state != current_system_state) { - case ARMING_STATE_INIT: + switch (new_system_state) { + case SYSTEM_STATE_INIT: - if (current_status->arming_state == ARMING_STATE_STANDBY) { + /* transitions back to INIT are possible for calibration */ + if (current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); - valid_transition = true; + ret = OK; } - break; - case ARMING_STATE_STANDBY: - - // TODO check for sensors - // XXX check if coming from reboot? - if (current_status->arming_state == ARMING_STATE_INIT) { + break; - if (current_status->flag_system_sensors_initialized) { - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); - } + case SYSTEM_STATE_MANUAL: - } else if (current_status->arming_state == ARMING_STATE_ARMED) { + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; + ret = OK; } - break; - case ARMING_STATE_ARMED: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); - valid_transition = true; - } break; - case ARMING_STATE_ABORT: + case SYSTEM_STATE_SEATBELT: - if (current_status->arming_state == ARMING_STATE_ARMED) { + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_AUTO) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); - valid_transition = true; + ret = OK; } - break; - - case ARMING_STATE_ERROR: - - if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_ABORT - || current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); - valid_transition = true; - } break; - case ARMING_STATE_REBOOT: + case SYSTEM_STATE_AUTO: - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_ERROR) { + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT) { - valid_transition = true; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + ret = OK; } break; - case ARMING_STATE_IN_AIR_RESTORE: + case SYSTEM_STATE_REBOOT: - if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); - valid_transition = true; - } - break; - default: - warnx("Unknown arming state"); - break; - } - } + /* from INIT you can go straight to REBOOT */ + if (current_system_state == SYSTEM_STATE_INIT) { - if (valid_transition) { - current_status->arming_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case HIL_STATE_OFF: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; + ret = OK; } break; - case HIL_STATE_ON: + case SYSTEM_STATE_IN_AIR_RESTORE: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + /* from INIT you can go straight to an IN AIR RESTORE */ + if (current_system_state == SYSTEM_STATE_INIT) { - current_status->flag_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; + ret = OK; } break; default: - warnx("Unknown hil state"); break; } } - if (valid_transition) { - current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); - } - return ret; } +///** +// * Transition from one arming state to another +// */ +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); +// +// if (current_status->arming_state == new_state) { +// warnx("Arming state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case ARMING_STATE_INIT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY) { +// +// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_STANDBY: +// +// // TODO check for sensors +// // XXX check if coming from reboot? +// if (current_status->arming_state == ARMING_STATE_INIT) { +// +// if (current_status->flag_system_sensors_initialized) { +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } else { +// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); +// } +// +// } else if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ARMED: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { +// current_status->flag_system_armed = true; +// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ABORT: +// +// if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ERROR: +// +// if (current_status->arming_state == ARMING_STATE_ARMED +// || current_status->arming_state == ARMING_STATE_ABORT +// || current_status->arming_state == ARMING_STATE_INIT) { +// +// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_REBOOT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_ERROR) { +// +// valid_transition = true; +// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); +// usleep(500000); +// up_systemreset(); +// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +// } +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// if (current_status->arming_state == ARMING_STATE_INIT) { +// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); +// valid_transition = true; +// } +// break; +// default: +// warnx("Unknown arming state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->arming_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); +// } +// +// return ret; +//} + +///** +// * Transition from one hil state to another +// */ +//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); +// +// if (current_status->hil_state == new_state) { +// warnx("Hil state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case HIL_STATE_OFF: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = false; +// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); +// valid_transition = true; +// } +// break; +// +// case HIL_STATE_ON: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = true; +// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); +// valid_transition = true; +// } +// break; +// +// default: +// warnx("Unknown hil state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->hil_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); +// } +// +// return ret; +//} + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf..57b3db8f1 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,9 +47,9 @@ #include #include -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 27a471f13..20cb25cc0 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,27 +60,48 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_STANDBY=0, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_DESCENT, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TAKEOFF -} navigation_state_t; + SYSTEM_STATE_INIT=0, + SYSTEM_STATE_MANUAL, + SYSTEM_STATE_SEATBELT, + SYSTEM_STATE_AUTO, + SYSTEM_STATE_REBOOT, + SYSTEM_STATE_IN_AIR_RESTORE +} system_state_t; typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ABORT, - ARMING_STATE_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE -} arming_state_t; + MANUAL_STANDBY = 0, + MANUAL_READY, + MANUAL_IN_AIR +} manual_state_t; + +typedef enum { + SEATBELT_STANDBY, + SEATBELT_READY, + SEATBELT, + SEATBELT_ASCENT, + SEATBELT_DESCENT, +} seatbelt_state_t; + +typedef enum { + AUTO_STANDBY, + AUTO_READY, + AUTO_LOITER, + AUTO_MISSION, + AUTO_RTL, + AUTO_TAKEOFF, + AUTO_LAND, +} auto_state_t; + +//typedef enum { +// ARMING_STATE_INIT = 0, +// ARMING_STATE_STANDBY, +// ARMING_STATE_ARMED_GROUND, +// ARMING_STATE_ARMED_AIRBORNE, +// ARMING_STATE_ERROR_GROUND, +// ARMING_STATE_ERROR_AIRBORNE, +// ARMING_STATE_REBOOT, +// ARMING_STATE_IN_AIR_RESTORE +//} arming_state_t; typedef enum { HIL_STATE_OFF = 0, @@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, + MODE_SWITCH_SEATBELT, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -114,26 +135,6 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; -//enum VEHICLE_FLIGHT_MODE { -// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ -// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ -// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ -// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -//}; -// -//enum VEHICLE_MANUAL_CONTROL_MODE { -// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ -// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ -// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -//}; -// -//enum VEHICLE_MANUAL_SAS_MODE { -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ -// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ -// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -//}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -180,8 +181,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current navigation state */ - arming_state_t arming_state; /**< current arming state */ + system_state_t system_state; /**< current system state */ +// arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -194,6 +195,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; bool flag_system_in_air_restore; /**< true if we can restore in mid air */ @@ -212,9 +214,6 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_land_requested; /**< true if land requested */ - bool flag_mission_activated; /**< true if in mission mode */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; -- cgit v1.2.3 From ebe0285ce7964ac1a81a65bae417e978cf366466 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 13:06:56 -0800 Subject: Checkpoint: navigation state machine as discussed with Lorenz --- Documentation/flight_mode_state_machine.odg | Bin 24059 -> 23463 bytes apps/commander/commander.c | 16 +- apps/commander/state_machine_helper.c | 357 +++++++++++++++++++++++----- apps/commander/state_machine_helper.h | 3 +- apps/controllib/fixedwing.cpp | 8 +- apps/mavlink/mavlink.c | 55 +++-- apps/uORB/topics/vehicle_status.h | 86 +++---- 7 files changed, 373 insertions(+), 152 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index a50366bcb..cbdad91c9 100644 Binary files a/Documentation/flight_mode_state_machine.odg and b/Documentation/flight_mode_state_machine.odg differ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8b9e7c49c..4e2b4907b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_STANDBY; + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; @@ -1857,19 +1857,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - - if (current_status.arming_state == ARMING_STATE_ARMED) { - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); - } - } - } + navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 2e4935471..e6344f1a8 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,60 +56,195 @@ /** * Transition from one sytem state to another */ -void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { int ret = ERROR; - system_state_t new_system_state; + navigation_state_t new_navigation_state; + /* do the navigation state update depending on the arming state */ + switch (current_status->arming_state) { - /* - * Firstly evaluate mode switch position - * */ + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_system_state = SYSTEM_STATE_MANUAL; + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL); - } + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT); - } else { - mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); - - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL; + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); + + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } + } } - } - } - /* - * Next up, check for - */ + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_NONE) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_RETURN) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + /* descent not possible without baro information, fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept LOITER if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_NONE) { + + if (current_status->flag_global_position_valid) { + new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Accept MISSION if there is a global position estimate and home position */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_MISSION) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Go to RTL or land if global position estimate and home position is available */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_RETURN + && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + + /* after RTL go to LAND */ + if (current_status->flag_system_returned_to_home) { + new_navigation_state = NAVIGATION_STATE_AUTO_LAND; + } else { + new_navigation_state = NAVIGATION_STATE_AUTO_RTL; + } + + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + } + break; + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } /* Update the system state in case it has changed */ - if (current_status->system_state != new_system_state) { + if (current_status->navigation_state != new_navigation_state) { /* Check if the transition is valid */ - if (system_state_update(current_status->system_state, new_system_state) == OK) { - current_status->system_state = new_system_state; + if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { + current_status->navigation_state = new_navigation_state; state_machine_publish(status_pub, current_status, mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "System state transition not valid"); @@ -123,74 +258,164 @@ void state_machine_update(int status_pub, struct vehicle_status_s *current_statu * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_system_state != current_system_state) { + if (new_navigation_state != current_navigation_state) { - switch (new_system_state) { - case SYSTEM_STATE_INIT: + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { + if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } + break; + case NAVIGATION_STATE_MANUAL: + + /* all transitions possible */ + ret = OK; break; - case SYSTEM_STATE_MANUAL: + case NAVIGATION_STATE_SEATBELT_STANDBY: - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { ret = OK; } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + ret = OK; + } break; - case SYSTEM_STATE_SEATBELT: + case NAVIGATION_STATE_SEATBELT_DESCENT: - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_AUTO) { + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { ret = OK; } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } break; - case SYSTEM_STATE_AUTO: + case NAVIGATION_STATE_AUTO_READY: - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT) { + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { ret = OK; } break; - case SYSTEM_STATE_REBOOT: + case NAVIGATION_STATE_AUTO_TAKEOFF: - /* from INIT you can go straight to REBOOT */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* only transitions from AUTO_READY */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } break; - case SYSTEM_STATE_IN_AIR_RESTORE: + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; - /* from INIT you can go straight to an IN AIR RESTORE */ - if (current_system_state == SYSTEM_STATE_INIT) { + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { ret = OK; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 57b3db8f1..1c0564d07 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,10 +47,11 @@ #include #include -int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9a6919535..584ca2306 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; +#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_MISSION) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position +#warning should be base on flags // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_MISSION || + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance @@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] : _manual.throttle; } - +#warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { // if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e25f1be27..34b267d56 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (v_status.flag_system_armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } - - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } +// +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } /** @@ -248,19 +252,30 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { + } else if (v_status.flag_system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_INIT) { + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 20cb25cc0..29baff34b 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,13 +60,20 @@ /* State Machine */ typedef enum { - SYSTEM_STATE_INIT=0, - SYSTEM_STATE_MANUAL, - SYSTEM_STATE_SEATBELT, - SYSTEM_STATE_AUTO, - SYSTEM_STATE_REBOOT, - SYSTEM_STATE_IN_AIR_RESTORE -} system_state_t; + NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_MANUAL_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT_STANDBY, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_AUTO_STANDBY, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_AUTO_TAKEOFF, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_LAND +} navigation_state_t; typedef enum { MANUAL_STANDBY = 0, @@ -75,50 +82,20 @@ typedef enum { } manual_state_t; typedef enum { - SEATBELT_STANDBY, - SEATBELT_READY, - SEATBELT, - SEATBELT_ASCENT, - SEATBELT_DESCENT, -} seatbelt_state_t; - -typedef enum { - AUTO_STANDBY, - AUTO_READY, - AUTO_LOITER, - AUTO_MISSION, - AUTO_RTL, - AUTO_TAKEOFF, - AUTO_LAND, -} auto_state_t; - -//typedef enum { -// ARMING_STATE_INIT = 0, -// ARMING_STATE_STANDBY, -// ARMING_STATE_ARMED_GROUND, -// ARMING_STATE_ARMED_AIRBORNE, -// ARMING_STATE_ERROR_GROUND, -// ARMING_STATE_ERROR_AIRBORNE, -// ARMING_STATE_REBOOT, -// ARMING_STATE_IN_AIR_RESTORE -//} arming_state_t; + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; typedef enum { HIL_STATE_OFF = 0, HIL_STATE_ON } hil_state_t; -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_SEATBELT, @@ -135,6 +112,17 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -181,8 +169,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - system_state_t system_state; /**< current system state */ -// arming_state_t arming_state; /**< current arming state */ + navigation_state_t navigation_state; /**< current system state */ + arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -203,7 +191,9 @@ struct vehicle_status_s bool flag_system_arming_requested; bool flag_system_disarming_requested; bool flag_system_reboot_requested; - bool flag_system_on_ground; + bool flag_system_returned_to_home; + + bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ -- cgit v1.2.3 From c056410f8439e760905cb50fe08c49c3a0b344e5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 18:10:34 -0800 Subject: Checkpoint: Added arming check function --- apps/commander/state_machine_helper.c | 55 +++++++++++++++++++++++++++++++++++ apps/commander/state_machine_helper.h | 2 ++ 2 files changed, 57 insertions(+) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e6344f1a8..0611bb52c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,6 +254,61 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state != current_arming_state) { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_arming_state == ARMING_STATE_STANDBY) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = OK; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + } + break; + default: + break; + } + } + return ret; +} + + + /* * This functions does not evaluate any input flags but only checks if the transitions * are valid. diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 1c0564d07..cf1fa80cd 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,4 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); + #endif /* STATE_MACHINE_HELPER_H_ */ -- cgit v1.2.3 From be7aeb754b85016e2609508d1c059797d5068ec1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 20:01:54 -0800 Subject: Checkpoint: Added flag checks inside arming state update --- apps/commander/state_machine_helper.c | 35 +++++++++++++++++++++++------------ apps/commander/state_machine_helper.h | 2 +- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 0611bb52c..e5ef00e93 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,49 +254,60 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } -int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { +int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_arming_state) { + if (new_arming_state != current_state->arming_state) { switch (new_arming_state) { case ARMING_STATE_INIT: /* allow going back from INIT for calibration */ - if (current_arming_state == ARMING_STATE_STANDBY) { + if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; } break; case ARMING_STATE_STANDBY: /* allow coming from INIT and disarming from ARMED */ - if (current_arming_state == ARMING_STATE_INIT - || current_arming_state == ARMING_STATE_ARMED) { - ret = OK; + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->flag_system_sensors_initialized) { + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } break; case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_arming_state == ARMING_STATE_STANDBY - || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ ret = OK; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ - if (current_arming_state == ARMING_STATE_ARMED) { + if (current_state->arming_state == ARMING_STATE_ARMED) { ret = OK; + + /* XXX conditions for an error state? */ } break; case ARMING_STATE_STANDBY_ERROR: /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_arming_state == ARMING_STATE_STANDBY - || current_arming_state == ARMING_STATE_INIT - || current_arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index cf1fa80cd..f2928db09 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,6 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); +int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); #endif /* STATE_MACHINE_HELPER_H_ */ -- cgit v1.2.3 From 36f9f8082e1ac676994cab0a6ee2c7a8344b0216 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 09:32:49 -0800 Subject: Checkpoint: Added flag checks inside navigation state update --- apps/commander/commander.c | 40 +- apps/commander/state_machine_helper.c | 856 ++++++++++++++++------------------ apps/commander/state_machine_helper.h | 8 +- apps/uORB/topics/vehicle_status.h | 1 + 4 files changed, 435 insertions(+), 470 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4e2b4907b..953ba7a1e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -804,7 +804,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -902,8 +902,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -924,8 +924,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -995,8 +995,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_accel_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX what if this fails + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + //XXX what if this fails + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1857,7 +1858,8 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - navigation_state_update(stat_pub, ¤t_status, mavlink_fd); + #warning do that + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { @@ -1880,7 +1882,7 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; } else { @@ -1892,7 +1894,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e5ef00e93..6c15bd725 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,206 +56,205 @@ /** * Transition from one sytem state to another */ -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int ret = ERROR; - navigation_state_t new_navigation_state; - - /* do the navigation state update depending on the arming state */ - switch (current_status->arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); - - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_NONE) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_RETURN) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - /* descent not possible without baro information, fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept LOITER if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_NONE) { - - if (current_status->flag_global_position_valid) { - new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; - } else { - mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Accept MISSION if there is a global position estimate and home position */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_MISSION) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Go to RTL or land if global position estimate and home position is available */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_RETURN - && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - - /* after RTL go to LAND */ - if (current_status->flag_system_returned_to_home) { - new_navigation_state = NAVIGATION_STATE_AUTO_LAND; - } else { - new_navigation_state = NAVIGATION_STATE_AUTO_RTL; - } - - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - } - break; - - case ARMING_STATE_ARMED_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - - - - /* Update the system state in case it has changed */ - if (current_status->navigation_state != new_navigation_state) { - - /* Check if the transition is valid */ - if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { - current_status->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - } else { - mavlink_log_critical(mavlink_fd, "System state transition not valid"); - } - } - - return; -} - -int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// int ret = ERROR; +// navigation_state_t new_navigation_state; +// +// /* do the navigation state update depending on the arming state */ +// switch (current_status->arming_state) { +// +// /* evaluate the navigation state when disarmed */ +// case ARMING_STATE_STANDBY: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// +// /* Accept AUTO if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); +// +// /* otherwise fallback to seatbelt or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// } +// } +// +// break; +// +// /* evaluate the navigation state when armed */ +// case ARMING_STATE_ARMED: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_NONE) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_RETURN) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// /* descent not possible without baro information, fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept LOITER if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_NONE) { +// +// if (current_status->flag_global_position_valid) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Accept MISSION if there is a global position estimate and home position */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_MISSION) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Go to RTL or land if global position estimate and home position is available */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_RETURN +// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// +// /* after RTL go to LAND */ +// if (current_status->flag_system_returned_to_home) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; +// } else { +// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; +// } +// +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// } +// break; +// +// case ARMING_STATE_ARMED_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_STANDBY_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_REBOOT: +// +// // XXX I don't think we should end up here +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// // XXX not sure what to do here +// break; +// default: +// break; +// } +// +// +// +// /* Update the system state in case it has changed */ +// if (current_status->navigation_state != new_navigation_state) { +// +// /* Check if the transition is valid */ +// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { +// current_status->navigation_state = new_navigation_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// } else { +// mavlink_log_critical(mavlink_fd, "System state transition not valid"); +// } +// } +// +// return; +//} +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ @@ -324,20 +323,20 @@ int check_arming_state_transition(struct vehicle_status_s *current_state, arming * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_navigation_state) { + if (new_navigation_state != current_state->navigation_state) { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } @@ -346,93 +345,139 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_MANUAL_STANDBY: /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_MANUAL: - /* all transitions possible */ - ret = OK; + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + } else { + ret = OK; + } break; case NAVIGATION_STATE_SEATBELT_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_STANDBY: /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + } else if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_READY: /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - ret = OK; + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } @@ -441,49 +486,75 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_AUTO_LOITER: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_MISSION: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state->flag_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_RTL: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_LAND: /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - ret = OK; + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + } } break; @@ -492,182 +563,71 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat } } + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + return ret; } -///** -// * Transition from one arming state to another -// */ -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); -// -// if (current_status->arming_state == new_state) { -// warnx("Arming state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case ARMING_STATE_INIT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY) { -// -// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_STANDBY: -// -// // TODO check for sensors -// // XXX check if coming from reboot? -// if (current_status->arming_state == ARMING_STATE_INIT) { -// -// if (current_status->flag_system_sensors_initialized) { -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } else { -// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); -// } -// -// } else if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ARMED: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { -// current_status->flag_system_armed = true; -// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ABORT: -// -// if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ERROR: -// -// if (current_status->arming_state == ARMING_STATE_ARMED -// || current_status->arming_state == ARMING_STATE_ABORT -// || current_status->arming_state == ARMING_STATE_INIT) { -// -// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_REBOOT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_ERROR) { -// -// valid_transition = true; -// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); -// usleep(500000); -// up_systemreset(); -// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -// } -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// if (current_status->arming_state == ARMING_STATE_INIT) { -// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); -// valid_transition = true; -// } -// break; -// default: -// warnx("Unknown arming state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->arming_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); -// } -// -// return ret; -//} -///** -// * Transition from one hil state to another -// */ -//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); -// -// if (current_status->hil_state == new_state) { -// warnx("Hil state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case HIL_STATE_OFF: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = false; -// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); -// valid_transition = true; -// } -// break; -// -// case HIL_STATE_ON: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = true; -// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); -// valid_transition = true; -// } -// break; -// -// default: -// warnx("Unknown hil state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->hil_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); -// } -// -// return ret; -//} +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index f2928db09..5b57cffb7 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -53,7 +53,9 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); -#endif /* STATE_MACHINE_HELPER_H_ */ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); + +#endif /* STATE_MACHINE_HELPER_H_ */ \ No newline at end of file diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 29baff34b..495542244 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -193,6 +193,7 @@ struct vehicle_status_s bool flag_system_reboot_requested; bool flag_system_returned_to_home; + bool flag_auto_mission_available; bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ -- cgit v1.2.3 From cbfa64b59eef8362494d0753ce3567e804f2d682 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 11:54:41 -0800 Subject: Checkpoint: Added switch handling --- apps/commander/commander.c | 162 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 160 insertions(+), 2 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 953ba7a1e..38d2ffc96 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1725,7 +1725,7 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + if (orb_check(gps_sub, &new_data)) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); @@ -1858,7 +1858,165 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - #warning do that + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ -- cgit v1.2.3 From 793b482e00013ea66bb1b0cdc0366bb720648938 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 15:52:13 -0800 Subject: Checkpoint: Navigation states and arming seem to work --- apps/commander/commander.c | 43 +++++++++++++++++++---------------- apps/commander/state_machine_helper.c | 25 +++++++++++++++----- apps/sensors/sensors.cpp | 2 +- 3 files changed, 43 insertions(+), 27 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 38d2ffc96..2784e77db 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -903,7 +903,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -925,7 +925,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -996,7 +996,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,8 +1647,9 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - //XXX what if this fails - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX fix for now + current_status.flag_system_sensors_initialized = true; + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1790,11 +1791,10 @@ int commander_thread_main(int argc, char *argv[]) * Check if manual control modes have to be switched */ if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + warnx("mode sw not finite"); /* no valid stick position, go to default */ - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ @@ -1806,6 +1806,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.mode_switch = MODE_SWITCH_AUTO; } else { + /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; } @@ -1869,7 +1870,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); } /* Try seatbelt or fallback to manual */ @@ -1879,7 +1880,7 @@ int commander_thread_main(int argc, char *argv[]) // fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); } } @@ -1892,7 +1893,7 @@ int commander_thread_main(int argc, char *argv[]) // or fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); } } } @@ -2040,7 +2041,8 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + printf("Try disarm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; } else { @@ -2052,7 +2054,8 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + printf("Try arm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 6c15bd725..ae7f2a1c1 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -255,10 +255,13 @@ //} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_state->arming_state) { + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { switch (new_arming_state) { case ARMING_STATE_INIT: @@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta default: break; } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + return ret; } @@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_state->navigation_state) { + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: @@ -561,13 +572,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current default: break; } - } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + + return ret; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ce404ee7e..b53de8c46 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -432,7 +432,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); -- cgit v1.2.3 From 0eca49a4f6d4a06868770c8b0c36094d889cb846 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 19:46:47 -0800 Subject: Checkpoint: Separated all bools in vehicle status into conditions and flags, they should be protected --- apps/commander/commander.c | 109 ++++++++++----------- apps/commander/state_machine_helper.c | 26 ++--- apps/drivers/blinkm/blinkm.cpp | 2 +- apps/drivers/px4io/px4io.cpp | 13 +-- apps/mavlink/mavlink.c | 4 +- apps/mavlink_onboard/mavlink.c | 2 +- .../multirotor_att_control_main.c | 7 +- apps/uORB/topics/vehicle_status.h | 43 ++++---- 8 files changed, 103 insertions(+), 103 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 94e344da1..2fdcf4ce3 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -323,8 +323,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_mag_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -559,8 +559,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_mag_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); close(sub_mag); } @@ -568,8 +568,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) { /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_gyro_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); const int calibration_count = 5000; @@ -621,8 +621,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[2] = gyro_offset[2] / calibration_count; /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_gyro_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { @@ -679,8 +679,8 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "keep it level and still"); /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_accel_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); const int calibration_count = 2500; @@ -787,8 +787,8 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) } /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_accel_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); close(sub_sensor_combined); } @@ -1022,7 +1022,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta break; case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && + if (current_status.flag_fmu_armed && ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { @@ -1327,7 +1327,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_system_armed = false; + current_status.flag_fmu_armed = false; + current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1341,13 +1342,13 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; + // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; // XXX for now just set sensors as initialized - current_status.flag_system_sensors_initialized = true; + current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1501,7 +1502,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!current_status.flag_system_armed) { + if (!current_status.flag_fmu_armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1654,7 +1655,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX fix for now - current_status.flag_system_sensors_initialized = true; + current_status.condition_system_sensors_initialized = true; arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1671,45 +1672,45 @@ int commander_thread_main(int argc, char *argv[]) */ /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; + current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_global_position_valid = false; + current_status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; + current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_local_position_valid = false; + current_status.condition_local_position_valid = false; } /* * Consolidate global position and local position valid flags * for vector flight mode. */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; - } else { - current_status.flag_vector_flight_mode_ok = false; - } + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } - /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid) { - state_changed = true; - } + // /* consolidate state change, flag as changed if required */ + // if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + // global_pos_valid != current_status.flag_global_position_valid || + // local_pos_valid != current_status.flag_local_position_valid) { + // state_changed = true; + // } /* * Mark the position of the first position lock as return to launch (RTL) @@ -1721,16 +1722,16 @@ int commander_thread_main(int argc, char *argv[]) * 2) The system has not aquired position lock before * 3) The system is not armed (on the ground) */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ - - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it + + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } if (orb_check(gps_sub, &new_data)) { @@ -1760,7 +1761,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { + && !current_status.flag_fmu_armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1876,7 +1877,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } /* Try seatbelt or fallback to manual */ @@ -1886,7 +1887,7 @@ int commander_thread_main(int argc, char *argv[]) // fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } } @@ -1899,7 +1900,7 @@ int commander_thread_main(int argc, char *argv[]) // or fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } } } @@ -2047,7 +2048,6 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - printf("Try disarm\n"); arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; @@ -2060,7 +2060,6 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - printf("Try arm\n"); arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; @@ -2156,13 +2155,13 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost_interval = 0; /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { + if (sp_offboard.armed && !current_status.flag_fmu_armed) { #warning fix this // update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ // update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_system_armed) { + } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { // update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index ae7f2a1c1..61ebe8c16 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -278,7 +278,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED) { /* sensors need to be initialized for STANDBY state */ - if (current_state->flag_system_sensors_initialized) { + if (current_state->condition_system_sensors_initialized) { ret = OK; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); @@ -392,7 +392,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); } else { ret = OK; @@ -416,7 +416,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); } else { ret = OK; @@ -439,7 +439,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); } else { ret = OK; @@ -458,9 +458,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - } else if (!current_state->flag_global_position_valid) { + } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); } else { ret = OK; @@ -504,9 +504,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); } else { ret = OK; @@ -524,7 +524,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ - if (!current_state->flag_auto_mission_available) { + if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); } else { ret = OK; @@ -542,9 +542,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); } else { ret = OK; @@ -559,9 +559,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); } else { ret = OK; diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 6aff27e4c..ceb2d987d 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -529,7 +529,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(vehicle_status_raw.flag_fmu_armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index adb894371..99f0f35b2 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -446,7 +446,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_system_armed); + } while (!status.flag_fmu_armed); /* regular boot, no in-air restart, init IO */ } else { @@ -658,11 +658,12 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_ARM_OK; } - if (vstatus.flag_vector_flight_mode_ok) { - set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; - } else { - clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; - } + // TODO fix this + // if (vstatus.flag_vector_flight_mode_ok) { + // set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + // } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 34b267d56..342328728 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -246,9 +246,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { + if (v_status.flag_preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index c5a1e82a8..fbfce7dc9 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_system_armed) { + if (v_status->flag_fmu_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index da7550f79..79ca9fe2d 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -247,7 +247,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + state.flag_fmu_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } @@ -291,7 +291,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + if (!flag_control_attitude_enabled && state.flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -395,7 +395,8 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_system_armed; + flag_system_armed = state.flag_fmu_armed; + // XXX add some logic to this perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 495542244..eba5a5047 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -183,31 +183,35 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + + bool condition_system_emergency; + bool condition_system_in_air_restore; /**< true if we can restore in mid air */ + bool condition_system_sensors_initialized; + bool condition_system_returned_to_home; + bool condition_auto_mission_available; + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_launch_position_valid; /**< indicates a valid launch position */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_local_position_valid; - bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ + bool flag_io_armed; /**< true is motors / actuators of IO are armed */ bool flag_system_emergency; - bool flag_system_in_air_restore; /**< true if we can restore in mid air */ - bool flag_system_sensors_initialized; - bool flag_system_arming_requested; - bool flag_system_disarming_requested; - bool flag_system_reboot_requested; - bool flag_system_returned_to_home; - - bool flag_auto_mission_available; - bool flag_auto_enabled; + bool flag_preflight_calibration; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - + bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - bool flag_preflight_accel_calibration; + + // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ + // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + // bool flag_preflight_accel_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ @@ -238,13 +242,10 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + // bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + // bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_valid_launch_position; /**< indicates a valid launch position */ - bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + }; /** -- cgit v1.2.3 From 34c84752d1ff7494529dfea8e72f3c090b451b3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Feb 2013 09:24:30 -0800 Subject: Checkpoint: Added control flags --- apps/commander/state_machine_helper.c | 576 ++++------------------------------ 1 file changed, 59 insertions(+), 517 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 61ebe8c16..742b7fe07 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,207 +53,6 @@ #include "state_machine_helper.h" -/** - * Transition from one sytem state to another - */ -//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -//{ -// int ret = ERROR; -// navigation_state_t new_navigation_state; -// -// /* do the navigation state update depending on the arming state */ -// switch (current_status->arming_state) { -// -// /* evaluate the navigation state when disarmed */ -// case ARMING_STATE_STANDBY: -// -// /* Always accept manual mode */ -// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// -// /* Accept SEATBELT if there is a local position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// /* or just fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// } -// -// /* Accept AUTO if there is a global position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); -// -// /* otherwise fallback to seatbelt or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// } -// } -// } -// -// break; -// -// /* evaluate the navigation state when armed */ -// case ARMING_STATE_ARMED: -// -// /* Always accept manual mode */ -// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// -// /* Accept SEATBELT if there is a local position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT -// && current_status->return_switch == RETURN_SWITCH_NONE) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// /* or just fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// -// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT -// && current_status->return_switch == RETURN_SWITCH_RETURN) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; -// } else { -// /* descent not possible without baro information, fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// -// /* Accept LOITER if there is a global position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_NONE -// && current_status->mission_switch == MISSION_SWITCH_NONE) { -// -// if (current_status->flag_global_position_valid) { -// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); -// -// /* otherwise fallback to SEATBELT or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// -// /* Accept MISSION if there is a global position estimate and home position */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_NONE -// && current_status->mission_switch == MISSION_SWITCH_MISSION) { -// -// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { -// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; -// } else { -// -// /* spit out what exactly is unavailable */ -// if (current_status->flag_global_position_valid) { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); -// } else if (current_status->flag_valid_home_position) { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); -// } -// -// /* otherwise fallback to SEATBELT or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// -// /* Go to RTL or land if global position estimate and home position is available */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_RETURN -// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { -// -// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { -// -// /* after RTL go to LAND */ -// if (current_status->flag_system_returned_to_home) { -// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; -// } else { -// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; -// } -// -// } else { -// -// /* spit out what exactly is unavailable */ -// if (current_status->flag_global_position_valid) { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); -// } else if (current_status->flag_valid_home_position) { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); -// } -// -// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// } -// break; -// -// case ARMING_STATE_ARMED_ERROR: -// -// // TODO work out fail-safe scenarios -// break; -// -// case ARMING_STATE_STANDBY_ERROR: -// -// // TODO work out fail-safe scenarios -// break; -// -// case ARMING_STATE_REBOOT: -// -// // XXX I don't think we should end up here -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// // XXX not sure what to do here -// break; -// default: -// break; -// } -// -// -// -// /* Update the system state in case it has changed */ -// if (current_status->navigation_state != new_navigation_state) { -// -// /* Check if the transition is valid */ -// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { -// current_status->navigation_state = new_navigation_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// } else { -// mavlink_log_critical(mavlink_fd, "System state transition not valid"); -// } -// } -// -// return; -//} - int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; @@ -269,6 +68,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; + current_state->flag_system_armed = false; } break; case ARMING_STATE_STANDBY: @@ -280,10 +80,10 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; + current_state->flag_system_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } - } break; case ARMING_STATE_ARMED: @@ -294,15 +94,17 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; + current_state->flag_system_armed = true; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ if (current_state->arming_state == ARMING_STATE_ARMED) { - ret = OK; - + /* XXX conditions for an error state? */ + ret = OK; + current_state->flag_system_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -311,6 +113,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; + current_state->flag_system_armed = false; } break; default: @@ -350,6 +153,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; + current_state->flag_control_rates_enabled = false; + current_state->flag_control_attitude_enabled = false; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } break; @@ -366,6 +173,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } } break; @@ -377,6 +188,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } break; @@ -396,6 +211,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -420,6 +239,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -443,6 +266,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -464,6 +291,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -481,6 +312,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -491,6 +326,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } break; @@ -510,6 +349,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -528,6 +371,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -548,6 +395,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -565,6 +416,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -683,38 +538,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //} -/* - * Private functions, update the state machine - */ -//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -//{ -// warnx("EMERGENCY HANDLER\n"); -// /* Depending on the current state go to one of the error states */ -// -// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); -// -// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { -// -// // DO NOT abort mission -// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); -// -// } else { -// warnx("Unknown system state: #%d\n", current_status->state_machine); -// } -//} - -//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -//{ -// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself -// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); -// -// } else { -// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); -// } -// -//} - // /* @@ -827,91 +650,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - ///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ // @@ -985,199 +723,3 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} - -#if 0 - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - - - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - -#endif -- cgit v1.2.3 From 0fe5aeb02c6ab0ac9c50f54d028cd5dde908e051 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 10:39:57 -0700 Subject: Checkpoint: More state machine fixes, commited to wrong branch and now copied over --- apps/ardrone_interface/ardrone_interface.c | 4 ++- apps/commander/commander.c | 32 ++++++++++++----- apps/commander/state_machine_helper.c | 34 ++++++++++++------ apps/mavlink/mavlink.c | 10 +++--- apps/mavlink_onboard/mavlink.c | 6 ++++ apps/mavlink_onboard/mavlink_receiver.c | 2 +- .../multirotor_att_control_main.c | 41 ++++++++++++---------- 7 files changed, 86 insertions(+), 43 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index aeaf830de..264041e65 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -331,7 +331,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* for now only spin if armed and immediately shut down * if in failsafe */ - if (armed.armed && !armed.lockdown) { + //XXX fix this + //if (armed.armed && !armed.lockdown) { + if (state.flag_fmu_armed) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2fdcf4ce3..a3e8fb745 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -60,13 +60,9 @@ #include #include #include -#include -#include -#include -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" #include #include + #include #include #include @@ -80,11 +76,19 @@ #include #include #include -#include +#include +#include +#include + +#include #include #include #include +#include + +#include "state_machine_helper.h" + /* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include @@ -101,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -#include + extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ @@ -120,6 +124,8 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + /* File descriptors */ static int leds; static int buzzer; @@ -1324,9 +1330,12 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; + current_status.flag_hil_enabled = false; current_status.flag_fmu_armed = false; current_status.flag_io_armed = false; // XXX read this from somewhere @@ -1542,6 +1551,13 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + /* update battery status */ orb_check(battery_sub, &new_data); if (new_data) { @@ -1565,7 +1581,7 @@ int commander_thread_main(int argc, char *argv[]) // current_status.state_machine == SYSTEM_STATE_AUTO || // current_status.state_machine == SYSTEM_STATE_MANUAL)) { // /* armed */ -// led_toggle(LED_BLUE); + led_toggle(LED_BLUE); } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not armed */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 742b7fe07..79394e2ba 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,19 +40,20 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include #include "state_machine_helper.h" - int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; @@ -68,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; case ARMING_STATE_STANDBY: @@ -80,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -94,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -104,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -113,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; default: @@ -157,6 +158,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = false; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } break; @@ -177,6 +179,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } } break; @@ -192,6 +195,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } break; @@ -215,6 +219,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -243,6 +248,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -270,6 +276,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -295,6 +302,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -316,6 +324,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -330,6 +339,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } break; @@ -353,6 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -375,6 +386,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -399,6 +411,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -420,6 +433,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -530,7 +544,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //void publish_armed_status(const struct vehicle_status_s *current_status) //{ // struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; +// armed.armed = current_status->flag_fmu_armed; // /* lock down actuators if required, only in HIL */ // armed.lockdown = (current_status->flag_hil_enabled) ? true : false; // orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -669,7 +683,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); //// //// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_system_armed) { +//// current_status->flag_fmu_armed) { //// //// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") //// @@ -694,7 +708,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only arm in standby state */ // // XXX REMOVE // if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { @@ -705,7 +719,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only disarm in ground ready */ // if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { // do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 342328728..c443d5a4a 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -230,10 +230,12 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } -// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { -// -// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; -// } + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } // // if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { // diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index fbfce7dc9..057a573b1 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -296,6 +296,12 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } + if (v_status->flag_control_velocity_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; + } + // switch (v_status->state_machine) { // case SYSTEM_STATE_PREFLIGHT: // if (v_status->flag_preflight_gyro_calibration || diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c index 0acbea675..2d406fb9f 100644 --- a/apps/mavlink_onboard/mavlink_receiver.c +++ b/apps/mavlink_onboard/mavlink_receiver.c @@ -328,4 +328,4 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -} \ No newline at end of file +} diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 79ca9fe2d..3329c5c48 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -150,7 +150,9 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; + bool flag_fmu_armed = false; + bool flag_control_position_enabled = false; + bool flag_control_velocity_enabled = false; /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; @@ -162,7 +164,6 @@ mc_thread_main(int argc, char *argv[]) param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -247,7 +248,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_system_armed) { + state.flag_fmu_armed != flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -313,20 +314,20 @@ mc_thread_main(int argc, char *argv[]) // * settings as well. // */ // -// /* only move setpoint if manual input is != 0 */ -// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// first_time_after_yaw_speed_control = true; -// -// } else { -// if (first_time_after_yaw_speed_control) { -// att_sp.yaw_body = att.yaw; -// first_time_after_yaw_speed_control = false; -// } -// -// control_yaw_position = true; -// } + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } // } // } @@ -337,6 +338,7 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; @@ -395,8 +397,9 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_fmu_armed; - // XXX add some logic to this + flag_control_position_enabled = state.flag_control_position_enabled; + flag_control_velocity_enabled = state.flag_control_velocity_enabled; + flag_fmu_armed = state.flag_fmu_armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ -- cgit v1.2.3 From 8e837dd1641941d670a30a6f4706a663efa03ca1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 11:09:49 -0700 Subject: Updated the state machine documentation --- Documentation/flight_mode_state_machine.odg | Bin 23463 -> 22619 bytes Documentation/flight_mode_state_machine.pdf | Bin 23031 -> 110086 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index cbdad91c9..2d119bd8c 100644 Binary files a/Documentation/flight_mode_state_machine.odg and b/Documentation/flight_mode_state_machine.odg differ diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf index cd234ada7..af41953ee 100644 Binary files a/Documentation/flight_mode_state_machine.pdf and b/Documentation/flight_mode_state_machine.pdf differ -- cgit v1.2.3 From 513e1c7398850a2a5b7ec3139d8c6dbc5a32c057 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 11:42:23 -0700 Subject: Added missing prototype --- apps/commander/commander.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index aaabe7f4b..28f099991 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -172,6 +172,9 @@ static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); +static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status); + + static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -801,7 +804,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ -- cgit v1.2.3 From 6039582928e199976074a4033336316c65737b66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Mar 2013 09:04:08 -0700 Subject: Compile error after merge fixed --- apps/controllib/fixedwing.cpp | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9edb84723..56b42253e 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -345,23 +345,21 @@ void BlockMultiModeBacksideAutopilot::update() #warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { -// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { #warning fix the different manual modes - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; -#warning please check whether this flag makes sense - } else if (_status.flag_control_attitude_enabled) { - - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; +#warning should be based on flags + } else if (_status.navigation_state == NAVIGATION_STATE_SEATBELT) { + + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; } // update all publications -- cgit v1.2.3 From b1e2011fcc068709574ddaab3d8bc831abcb7de8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Mar 2013 10:36:23 -0700 Subject: Added feedback before rebooting --- apps/systemcmds/reboot/reboot.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c index 47d3cd948..cc380f94b 100644 --- a/apps/systemcmds/reboot/reboot.c +++ b/apps/systemcmds/reboot/reboot.c @@ -47,6 +47,9 @@ __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { + printf("Rebooting now...\n"); + fflush(stdout); + usleep(5000); up_systemreset(); } -- cgit v1.2.3 From 3665d7b86fdbd8489099dafb436aaddcb816efde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Mar 2013 14:53:54 -0700 Subject: Improved command handling, added a low priority task and various fixes --- apps/commander/commander.c | 608 +++++++++++++++++----------------- apps/commander/state_machine_helper.c | 16 + 2 files changed, 312 insertions(+), 312 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 388ba1964..27abb9d45 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -143,16 +143,26 @@ static int daemon_task; /**< Handle of daemon task / thread */ static struct vehicle_status_s current_status; static orb_advert_t stat_pub; -/* XXX ? */ -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -/* XXX ? */ +/* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + /* pthread loops */ static void *orb_receive_loop(void *arg); +static void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -161,18 +171,23 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); -static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status); +int buzzer_init(void); +void buzzer_deinit(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +void do_reboot(void); +void do_gyro_calibration(void); +void do_mag_calibration(void); +void do_rc_calibration(void); +void do_accel_calibration(void); +void do_airspeed_calibration(void); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); @@ -184,7 +199,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u /** * Print the correct usage. */ -static void usage(const char *reason); +void usage(const char *reason); /** * Sort calibration values. @@ -196,7 +211,7 @@ static void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -static int buzzer_init() +int buzzer_init() { buzzer = open("/dev/tone_alarm", O_WRONLY); @@ -208,13 +223,13 @@ static int buzzer_init() return 0; } -static void buzzer_deinit() +void buzzer_deinit() { close(buzzer); } -static int led_init() +int led_init() { leds = open(LED_DEVICE_PATH, 0); @@ -231,12 +246,12 @@ static int led_init() return 0; } -static void led_deinit() +void led_deinit() { close(leds); } -static int led_toggle(int led) +int led_toggle(int led) { static int last_blue = LED_ON; static int last_amber = LED_ON; @@ -248,59 +263,50 @@ static int led_toggle(int led) return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); } -static int led_on(int led) +int led_on(int led) { return ioctl(leds, LED_ON, led); } -static int led_off(int led) +int led_off(int led) { return ioctl(leds, LED_OFF, led); } -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +void tune_error() { - -#warning add alarms for state transitions - /* Trigger alarm if going into any error state */ -// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || -// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { -// ioctl(buzzer, TONE_SET_ALARM, 0); -// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); -// } - - /* Trigger neutral on arming / disarming */ -// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { -// ioctl(buzzer, TONE_SET_ALARM, 0); -// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); -// } - - /* Trigger Tetris on being bored */ - - return 0; + ioctl(buzzer, TONE_SET_ALARM, 2); } -void tune_confirm(void) +void tune_positive() { ioctl(buzzer, TONE_SET_ALARM, 3); } -void tune_error(void) +void tune_neutral() { - /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +void tune_negative() { + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +void do_reboot() +{ + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +} + + +void do_rc_calibration() +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); return; @@ -311,7 +317,6 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; @@ -320,22 +325,21 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ - /* auto-save to EEPROM */ + /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); } + tune_positive(); + mavlink_log_info(mavlink_fd, "trim calibration done"); } -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +void do_mag_calibration() { - - /* set to mag calibration mode */ - // status->flag_preflight_mag_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -350,15 +354,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* limit update rate to get equally spaced measurements over time (in ms) */ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); /* erase old calibration */ @@ -404,10 +399,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) return; } - tune_confirm(); - sleep(2); - tune_confirm(); - while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -421,9 +412,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); - tune_confirm(); + tune_neutral(); axis_deadline += calibration_interval / 3; } @@ -559,28 +550,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); } - /* disable calibration mode */ - // status->flag_preflight_mag_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - close(sub_mag); } -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +void do_gyro_calibration() { - /* set to gyro calibration mode */ - // status->flag_preflight_gyro_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); const int calibration_count = 5000; @@ -631,10 +613,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; - /* exit gyro calibration mode */ - // status->flag_preflight_gyro_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) @@ -671,10 +649,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { @@ -684,14 +659,10 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_accel_calibration(int status_pub, struct vehicle_status_s *status) +void do_accel_calibration() { - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it level and still"); - /* set to accel calibration mode */ - // status->flag_preflight_accel_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + /* give directions */ + mavlink_log_info(mavlink_fd, "accel calibration starting, keep it level"); const int calibration_count = 2500; @@ -787,28 +758,19 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); } - /* exit accel calibration mode */ - // status->flag_preflight_accel_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - close(sub_sensor_combined); } -static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +void do_airspeed_calibration() { - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it still"); + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); const int calibration_count = 2500; @@ -857,11 +819,7 @@ static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *sta //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); @@ -870,16 +828,11 @@ static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *sta close(sub_differential_pressure); } - - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command handling */ - tune_confirm(); - /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: @@ -935,12 +888,16 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + do_reboot(); } else { /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); } } } @@ -971,234 +928,155 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // /* preflight calibration */ case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } +#if 0 /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again - // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - // - // // XXX implement this - // mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - // tune_confirm(); - // - // /* back to standby state */ - // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - // - // } else { - // mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - // result = VEHICLE_CMD_RESULT_DENIED; - // } - handled = true; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } +#endif +#if 0 /* trim calibration */ if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again - // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - // mavlink_log_info(mavlink_fd, "starting trim cal"); - // tune_confirm(); - // do_rc_calibration(status_pub, ¤t_status); - // mavlink_log_info(mavlink_fd, "finished trim cal"); - // tune_confirm(); - // - // /* back to standby state */ - // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - // - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // - // } else { - // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - // result = VEHICLE_CMD_RESULT_DENIED; - // } - handled = true; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } +#endif /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting acc cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished acc cal"); - tune_confirm(); - // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - - handled = true; } /* airspeed calibration */ if ((int)(cmd->param6) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished airspeed cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } break; case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_fmu_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); - - } else { - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ + if (((int)(cmd->param1)) == 0) { - if (((int)(cmd->param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - result = VEHICLE_CMD_RESULT_FAILED; - } + } else if (((int)(cmd->param1)) == 1) { - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } - break; + } break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); } break; } @@ -1207,10 +1085,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); + + tune_negative(); } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); + + tune_positive(); } /* send any requested ACKs */ @@ -1324,8 +1204,7 @@ float battery_remaining_estimate_voltage(float voltage) return ret; } -static void -usage(const char *reason) +void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -1405,6 +1284,7 @@ int commander_thread_main(int argc, char *argv[]) /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; pthread_t subsystem_info_thread; + pthread_t commander_low_prio_thread; /* initialize */ if (led_init() != 0) { @@ -1477,6 +1357,16 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&subsystem_info_attr, 2048); pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + /* Start monitoring loop */ uint16_t counter = 0; @@ -1487,12 +1377,14 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint8_t low_voltage_counter = 0; uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; float bat_remain = 1.0f; uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; + /* XXX what exactly is this for? */ + uint64_t last_print_time = 0; + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1562,7 +1454,9 @@ int commander_thread_main(int argc, char *argv[]) thread_running = true; uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; bool state_changed = true; bool param_init_forced = true; @@ -1727,7 +1621,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { /* For less than 20%, start be slightly annoying at 1 Hz */ ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); + tune_positive(); } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -1916,7 +1810,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ home_position_set = true; - tune_confirm(); + tune_positive(); } } @@ -2206,13 +2100,19 @@ int commander_thread_main(int argc, char *argv[]) current_status.rc_signal_lost_interval = 0; } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + last_print_time = hrt_absolute_time(); } @@ -2270,7 +2170,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_control_manual_enabled = false; current_status.flag_control_offboard_enabled = true; state_changed = true; - tune_confirm(); + tune_positive(); mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); @@ -2278,7 +2178,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); state_changed = true; - tune_confirm(); + tune_positive(); } } @@ -2298,7 +2198,6 @@ int commander_thread_main(int argc, char *argv[]) } } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { @@ -2314,7 +2213,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost_interval > 100000) { current_status.offboard_control_signal_lost = true; current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); + tune_positive(); /* kill motors after timeout */ if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { @@ -2352,6 +2251,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; + /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -2360,6 +2260,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ // pthread_join(command_handling_thread, NULL); pthread_join(subsystem_info_thread, NULL); + pthread_join(commander_low_prio_thread, NULL); /* close fds */ led_deinit(); @@ -2377,3 +2278,86 @@ int commander_thread_main(int argc, char *argv[]) return 0; } + + +static void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + +// do_baro_calibration(); + + case LOW_PRIO_TASK_RC_CALIBRATION: + +// do_rc_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 79394e2ba..ba01f8410 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -117,6 +117,22 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->flag_fmu_armed = false; } break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + current_state->flag_fmu_armed = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; default: break; } -- cgit v1.2.3 From f8e5b0cb0e01f89e7cffcdb97f4d5daaae499f72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Mar 2013 10:44:49 -0700 Subject: Small fixes and comments --- apps/commander/commander.c | 48 +++++++++++++++++++--------------------------- 1 file changed, 20 insertions(+), 28 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27abb9d45..bde4f2856 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -91,8 +91,6 @@ #include "state_machine_helper.h" - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include #include #include @@ -161,8 +159,8 @@ enum { /* pthread loops */ -static void *orb_receive_loop(void *arg); -static void *commander_low_prio_loop(void *arg); +void *orb_receive_loop(void *arg); +void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -190,7 +188,7 @@ void do_accel_calibration(void); void do_airspeed_calibration(void); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -1101,7 +1099,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal { /* Set thread name */ prctl(PR_SET_NAME, "commander orb rcv", getpid()); @@ -1669,8 +1667,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - // XXX fix for now - current_status.condition_system_sensors_initialized = true; + // XXX check for sensors arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1730,7 +1727,6 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; // } - // XXX why is this needed? /* consolidate state change, flag as changed if required */ if (global_pos_valid != current_status.condition_global_position_valid || local_pos_valid != current_status.condition_local_position_valid || @@ -2030,12 +2026,12 @@ int commander_thread_main(int argc, char *argv[]) case ARMING_STATE_ARMED_ERROR: - // TODO work out fail-safe scenarios + // XXX work out fail-safe scenarios break; case ARMING_STATE_STANDBY_ERROR: - // TODO work out fail-safe scenarios + // XXX work out fail-safe scenarios break; case ARMING_STATE_REBOOT: @@ -2050,9 +2046,6 @@ int commander_thread_main(int argc, char *argv[]) default: break; } - - // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); - /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; @@ -2068,11 +2061,11 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - // ) && - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; @@ -2186,15 +2179,15 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; + // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_fmu_armed) { -#warning fix this -// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ -// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { -// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } } else { @@ -2229,7 +2222,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); - // XXX what is this? + // XXX this is missing /* If full run came back clean, transition to standby */ // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && // current_status.flag_preflight_gyro_calibration == false && @@ -2241,8 +2234,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { -#warning fix this -// publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2280,7 +2272,7 @@ int commander_thread_main(int argc, char *argv[]) } -static void *commander_low_prio_loop(void *arg) +void *commander_low_prio_loop(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "commander low prio", getpid()); -- cgit v1.2.3 From 3cb3fa224baffcbb5ea228287f3d5b4f226b813b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Mar 2013 15:37:13 -0700 Subject: Small fixes again --- apps/commander/commander.c | 6 +++++- apps/mavlink/mavlink.c | 6 +----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index bde4f2856..77853aa7a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1082,7 +1082,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* supported command handling stop */ if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { tune_negative(); @@ -1828,16 +1829,19 @@ int commander_thread_main(int argc, char *argv[]) /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; + printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; + printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; + printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 084ab0d32..2643cbf7b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -236,11 +236,6 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } -// -// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { -// -// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; -// } /** @@ -577,6 +572,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } -- cgit v1.2.3 From 83e356fda41b0936d924e4e74673789bfdd0b29c Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 27 Mar 2013 16:12:29 +0400 Subject: Initial version of position_estimator_inav added --- apps/position_estimator_inav/Makefile | 47 +++ .../kalman_filter_inertial.c | 21 ++ .../kalman_filter_inertial.h | 5 + .../position_estimator_inav_main.c | 349 +++++++++++++++++++++ .../position_estimator_inav_params.c | 59 ++++ .../position_estimator_inav_params.h | 63 ++++ apps/position_estimator_inav/sounds.c | 40 +++ apps/position_estimator_inav/sounds.h | 16 + nuttx/configs/px4fmu/nsh/appconfig | 1 + 9 files changed, 601 insertions(+) create mode 100644 apps/position_estimator_inav/Makefile create mode 100644 apps/position_estimator_inav/kalman_filter_inertial.c create mode 100644 apps/position_estimator_inav/kalman_filter_inertial.h create mode 100644 apps/position_estimator_inav/position_estimator_inav_main.c create mode 100644 apps/position_estimator_inav/position_estimator_inav_params.c create mode 100644 apps/position_estimator_inav/position_estimator_inav_params.h create mode 100644 apps/position_estimator_inav/sounds.c create mode 100644 apps/position_estimator_inav/sounds.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile new file mode 100644 index 000000000..4906f2d97 --- /dev/null +++ b/apps/position_estimator_inav/Makefile @@ -0,0 +1,47 @@ +############################################################################ +# +# 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 position estimator +# + +APPNAME = position_estimator_inav +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +CSRCS = position_estimator_inav_main.c \ + position_estimator_inav_params.c \ + kalman_filter_inertial.c \ + sounds.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c new file mode 100644 index 000000000..7de06cb44 --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -0,0 +1,21 @@ +#include "kalman_filter_inertial.h" + +void kalman_filter_inertial_predict(float dt, float x[3]) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { + float y[2]; + // y = z - x + for (int i = 0; i < 2; i++) { + y[i] = z[i] - x[i]; + } + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 2; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h new file mode 100644 index 000000000..e6bbf1315 --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -0,0 +1,5 @@ +#include + +void kalman_filter_inertial_predict(float dt, float x[3]); + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c new file mode 100644 index 000000000..e28be5b51 --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * 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 position_estimator_inav_main.c + * Model-identification based position estimator for multirotors + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "position_estimator_inav_params.h" +//#include +#include "sounds.h" +#include +#include "kalman_filter_inertial.h" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int position_estimator_inav_task; /**< Handle of deamon task / thread */ + +__EXPORT int position_estimator_inav_main(int argc, char *argv[]); + +int position_estimator_inav_thread_main(int argc, char *argv[]); +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void usage(const char *reason) { + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + exit(1); +} + + /** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int position_estimator_inav_main(int argc, char *argv[]) { + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("position_estimator_inav already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + position_estimator_inav_task = task_spawn("position_estimator_inav", + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL ); + exit(0); + } + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tposition_estimator_inav is running\n"); + } else { + printf("\tposition_estimator_inav not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/**************************************************************************** + * main + ****************************************************************************/ +int position_estimator_inav_thread_main(int argc, char *argv[]) { + /* welcome user */ + printf("[position_estimator_inav] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + + /* initialize values */ + static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + static float x_est[3] = { 0.0f, 0.0f, 0.0f }; + static float y_est[3] = { 0.0f, 0.0f, 0.0f }; + static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + const static float dT_const_120 = 1.0f / 120.0f; + const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; + + int baro_loop_cnt = 0; + int baro_loop_end = 70; /* measurement for 1 second */ + float baro_alt0 = 0.0f; /* to determin while start up */ + float rho0 = 1.293f; /* standard pressure */ + const static float const_earth_gravity = 9.81f; + + static double lat_current = 0.0d; //[°]] --> 47.0 + static double lon_current = 0.0d; //[°]] -->8.5 + static double alt_current = 0.0d; //[m] above MSL + + /* declare and safely initialize all structs */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_local_position_s local_pos_est; + memset(&local_pos_est, 0, sizeof(local_pos_est)); + + /* subscribe */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* advertise */ + orb_advert_t local_pos_est_pub = orb_advertise( + ORB_ID(vehicle_local_position), &local_pos_est); + + struct position_estimator_inav_params pos_inav_params; + struct position_estimator_inav_param_handles pos_inav_param_handles; + /* initialize parameter handles */ + parameters_init(&pos_inav_param_handles); + + bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ + /* FIRST PARAMETER READ at START UP*/ + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ + /* FIRST PARAMETER UPDATE */ + parameters_update(&pos_inav_param_handles, &pos_inav_params); + /* END FIRST PARAMETER UPDATE */ + /* wait until gps signal turns valid, only then can we initialize the projection */ + struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events = + POLLIN } }; + + while (gps.fix_type < 3) { + + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (fds_init[0].revents & POLLIN) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, + &gps); + } + } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + printf("[pos_est1D] wait for GPS fix type 3\n"); + } + printcounter++; + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + lat_current = ((double) (gps.lat)) * 1e-7; + lon_current = ((double) (gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", + lat_current, lon_current); + uint64_t last_time = 0; + thread_running = true; + + /** main loop */ + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { + .fd = vehicle_status_sub, .events = POLLIN }, { .fd = + vehicle_attitude_sub, .events = POLLIN }, { .fd = + sensor_combined_sub, .events = POLLIN }, { .fd = + vehicle_gps_position_sub, .events = POLLIN }, }; + while (!thread_should_exit) { + int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate + if (ret < 0) { + /* poll error */ + } else { + /* parameter update */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, + &update); + /* update parameters */ + parameters_update(&pos_inav_param_handles, &pos_inav_params); + //r = pos_inav_params.r; + } + /* vehicle status */ + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + } + /* vehicle attitude */ + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + } + /* sensor combined */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, + &sensor); + } + /* vehicle GPS position */ + if (fds[4].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, + &gps); + static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + /* Project gps lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double) (gps.lat)) * 1e-7, + ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), + &(local_pos_gps[1])); + local_pos_gps[2] = (float) (gps.alt * 1e-3); + } + // barometric pressure estimation at start up + if (!local_flag_baroINITdone) { + // mean calculation over several measurements + if (baro_loop_cnt < baro_loop_end) { + baro_alt0 += sensor.baro_alt_meter; + baro_loop_cnt++; + } else { + baro_alt0 /= (float) (baro_loop_cnt); + local_flag_baroINITdone = true; + char *baro_m_start = "barometer initialized with alt0 = "; + char p0_char[15]; + sprintf(p0_char, "%8.2f", baro_alt0 / 100); + char *baro_m_end = " m"; + char str[80]; + strcpy(str, baro_m_start); + strcat(str, p0_char); + strcat(str, baro_m_end); + mavlink_log_info(mavlink_fd, str); + } + } + /* TODO convert accelerations from UAV frame to NED frame */ + float accel_NED[3]; + accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; + /* prediction */ + kalman_filter_inertial_predict(dT_const_120, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // pos, accel + bool use_z[2] = { false, true }; + if (local_flag_baroINITdone) { + z_meas[0] = sensor.baro_alt_meter - baro_alt0; + use_z[0] = true; + } + z_meas[1] = accel_NED[2]; + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, k, use_z); + printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]); + printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); + + local_pos_est.x = 0.0; + local_pos_est.vx = 0.0; + local_pos_est.y = 0.0; + local_pos_est.vy = 0.0; + local_pos_est.z = z_est[0]; + local_pos_est.vz = z_est[1]; + local_pos_est.timestamp = hrt_absolute_time(); + if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) + && (isfinite(local_pos_est.y)) + && (isfinite(local_pos_est.vy)) + && (isfinite(local_pos_est.z)) + && (isfinite(local_pos_est.vz))) { + orb_publish(ORB_ID( + vehicle_local_position), local_pos_est_pub, + &local_pos_est); + } + } /* end of poll return value check */ + } + + printf("[pos_est_inav] exiting.\n"); + mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting"); + thread_running = false; + return 0; +} diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c new file mode 100644 index 000000000..5567fd2cf --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli +* 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 position_estimator_inav_params.c + * + * Parameters for position_estimator_inav + */ + +#include "position_estimator_inav_params.h" + +/* Kalman Filter covariances */ +/* gps measurement noise standard deviation */ +PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); + +int parameters_init(struct position_estimator_inav_param_handles *h) +{ + h->r = param_find("POS_EST_R"); + return OK; +} + +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ + param_get(h->r, &(p->r)); + return OK; +} diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h new file mode 100644 index 000000000..c45ca8135 --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * 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 position_estimator_inav_params.h + * + * Parameters for Position Estimator + */ + +#include + +struct position_estimator_inav_params { + float r; +}; + +struct position_estimator_inav_param_handles { + param_t r; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct position_estimator_inav_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c new file mode 100644 index 000000000..4f7b05fea --- /dev/null +++ b/apps/position_estimator_inav/sounds.c @@ -0,0 +1,40 @@ +/* + * sounds.c + * + * Created on: Feb 26, 2013 + * Author: samuezih + */ + +#include +#include +#include + + +static int buzzer; + +int sounds_init(void) +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +void sounds_deinit(void) +{ + close(buzzer); +} + +void tune_tetris(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 6); +} + +void tune_sonar(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 7); +} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h new file mode 100644 index 000000000..356e42169 --- /dev/null +++ b/apps/position_estimator_inav/sounds.h @@ -0,0 +1,16 @@ +/* + * sounds.h + * + * Created on: Feb 26, 2013 + * Author: samuezih + */ + +#ifndef SOUNDS_H_ +#define SOUNDS_H_ + +int sounds_init(void); +void sounds_deinit(void); +void tune_tetris(void); +void tune_sonar(void); + +#endif /* SOUNDS_H_ */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312..c92e29930 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -101,6 +101,7 @@ CONFIGURED_APPS += multirotor_pos_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator +CONFIGURED_APPS += position_estimator_inav CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry endif -- cgit v1.2.3 From b837692d48e6dcc568bdf7009f515504c0772cf8 Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 Mar 2013 21:35:38 +0400 Subject: Test rotation matrix --- .../position_estimator_inav_main.c | 110 ++++++++++++--------- 1 file changed, 64 insertions(+), 46 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index e28be5b51..774125e83 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -154,6 +154,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { const static float dT_const_120 = 1.0f / 120.0f; const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; + bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -200,40 +201,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events = - POLLIN } }; + if (use_gps) { + struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, + .events = POLLIN } }; - while (gps.fix_type < 3) { + while (gps.fix_type < 3) { - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ - if (fds_init[0].revents & POLLIN) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, - &gps); + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (fds_init[0].revents & POLLIN) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + orb_copy(ORB_ID(vehicle_gps_position), + vehicle_gps_position_sub, &gps); + } } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + printf("[pos_est1D] wait for GPS fix type 3\n"); + } + printcounter++; } - static int printcounter = 0; - if (printcounter == 100) { - printcounter = 0; - printf("[pos_est1D] wait for GPS fix type 3\n"); - } - printcounter++; - } - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + lat_current = ((double) (gps.lat)) * 1e-7; + lon_current = ((double) (gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + } printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); uint64_t last_time = 0; @@ -245,6 +248,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN }, }; + printf("[position_estimator_inav] main loop started\n"); + static int printatt_counter = 0; while (!thread_should_exit) { int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate if (ret < 0) { @@ -262,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } /* vehicle status */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, + &vehicle_status); } /* vehicle attitude */ if (fds[2].revents & POLLIN) { @@ -270,20 +276,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } /* sensor combined */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, - &sensor); + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } - /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, - &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + if (use_gps) { + /* vehicle GPS position */ + if (fds[4].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), + vehicle_gps_position_sub, &gps); + static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + /* Project gps lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double) (gps.lat)) * 1e-7, + ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), + &(local_pos_gps[1])); + local_pos_gps[2] = (float) (gps.alt * 1e-3); + } } // barometric pressure estimation at start up if (!local_flag_baroINITdone) { @@ -320,9 +327,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { z_meas[1] = accel_NED[2]; /* correction */ kalman_filter_inertial_update(z_est, z_meas, k, use_z); - printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]); - printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); - + if (printatt_counter == 100) { + printatt_counter = 0; + printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", + att.pitch, att.roll, att.yaw); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[0][0], att.R[0][1], att.R[0][2]); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[1][0], att.R[1][1], att.R[1][2]); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[2][0], att.R[2][1], att.R[2][2]); + printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", + z_est[0], z_est[1], z_est[2]); + } + printatt_counter++; local_pos_est.x = 0.0; local_pos_est.vx = 0.0; local_pos_est.y = 0.0; -- cgit v1.2.3 From fb663bbb0c02ab6d6b6e1b360f5833028cd25bdd Mon Sep 17 00:00:00 2001 From: Anton Date: Fri, 29 Mar 2013 13:08:56 +0400 Subject: Acceleration convertion from UAV frame to NED frame --- .../position_estimator_inav_main.c | 75 +++++++++++++--------- 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 774125e83..4277fe19b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -87,18 +87,19 @@ static void usage(const char *reason); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -147,7 +148,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f, + 0.99f } }; static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -167,7 +169,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + memset(&vehicle_status, 0, sizeof(vehicle_status)); + /* make sure that baroINITdone = false */ struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; @@ -239,19 +242,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - uint64_t last_time = 0; + hrt_abstime last_time = 0; thread_running = true; /** main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN }, }; + struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); static int printatt_counter = 0; while (!thread_should_exit) { - int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate + int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate if (ret < 0) { /* poll error */ } else { @@ -271,16 +270,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &vehicle_status); } /* vehicle attitude */ - if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - } + //if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + //} /* sensor combined */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } if (use_gps) { /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { + //if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -290,7 +289,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); - } + //} } // barometric pressure estimation at start up if (!local_flag_baroINITdone) { @@ -312,16 +311,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, str); } } - /* TODO convert accelerations from UAV frame to NED frame */ - float accel_NED[3]; - accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; + + hrt_abstime t = hrt_absolute_time(); + float dt = (t - last_time) / 1000000.0; + last_time = t; + /* convert accelerations from UAV frame to NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, 0.0f }; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + accel_NED[2] += const_earth_gravity; /* prediction */ - kalman_filter_inertial_predict(dT_const_120, z_est); + kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // pos, accel bool use_z[2] = { false, true }; if (local_flag_baroINITdone) { - z_meas[0] = sensor.baro_alt_meter - baro_alt0; + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt use_z[0] = true; } z_meas[1] = accel_NED[2]; @@ -329,14 +337,23 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_update(z_est, z_meas, k, use_z); if (printatt_counter == 100) { printatt_counter = 0; + printf("[pos_est_inav] dt = %.6f\n", dt); printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", att.pitch, att.roll, att.yaw); + printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n", + sensor.accelerometer_m_s2[0], + sensor.accelerometer_m_s2[1], + sensor.accelerometer_m_s2[2]); + printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n", + accel_NED[0], accel_NED[1], accel_NED[2]); + /* printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[0][0], att.R[0][1], att.R[0][2]); printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[1][0], att.R[1][1], att.R[1][2]); printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[2][0], att.R[2][1], att.R[2][2]); + */ printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); } -- cgit v1.2.3 From 382af2b52d98de82c5cc0953e2c2ba7acf65501f Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 01:54:04 +0400 Subject: Acceleration vector transform implemented. Accelerometer calibration procedure defined but not implemented yet. --- apps/position_estimator_inav/Makefile | 1 + .../acceleration_transform.c | 93 +++++++++ .../acceleration_transform.h | 15 ++ .../position_estimator_inav_main.c | 214 +++++++++++++-------- 4 files changed, 241 insertions(+), 82 deletions(-) create mode 100644 apps/position_estimator_inav/acceleration_transform.c create mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 4906f2d97..e5f3b4b42 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,6 +42,7 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ + acceleration_transform.c \ sounds.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c new file mode 100644 index 000000000..9080c4e20 --- /dev/null +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -0,0 +1,93 @@ +/* + * acceleration_transform.c + * + * Created on: 30.03.2013 + * Author: Anton Babushkin + * + * Transform acceleration vector to true orientation and scale + * + * * * * Model * * * + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in UAV frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - true acceleration offset vector, don't use sensors offset because + * it's caused not only by real offset but also by sensor rotation + * + * * * * Calibration * * * + * + * Tests + * + * accel_corr_x_p = [ g 0 0 ]^T // positive x + * accel_corr_x_n = [ -g 0 0 ]^T // negative x + * accel_corr_y_p = [ 0 g 0 ]^T // positive y + * accel_corr_y_n = [ 0 -g 0 ]^T // negative y + * accel_corr_z_p = [ 0 0 g ]^T // positive z + * accel_corr_z_n = [ 0 0 -g ]^T // negative z + * + * 6 tests * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs = (accel_raw_x_p + accel_raw_x_n + + * accel_raw_y_p + accel_raw_y_n + + * accel_raw_z_p + accel_raw_z_n) / 6 + * + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + + * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + + * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * A x = b + * + * x = [ accel_T[0][0] ] + * | accel_T[0][1] | + * | accel_T[0][2] | + * | accel_T[1][0] | + * | accel_T[1][1] | + * | accel_T[1][2] | + * | accel_T[2][0] | + * | accel_T[2][1] | + * [ accel_T[2][2] ] + * + * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough + * | accel_corr_x_p[1] | + * | accel_corr_x_p[2] | + * | accel_corr_y_p[0] | + * | accel_corr_y_p[1] | + * | accel_corr_y_p[2] | + * | accel_corr_z_p[0] | + * | accel_corr_z_p[1] | + * [ accel_corr_z_p[2] ] + * + * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis + * ... + * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] + * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | + * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | + * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | + * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | + * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | + * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | + * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | + * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ] + * + * x = A^-1 b + */ + +#include "acceleration_transform.h" + +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) { + for (int i = 0; i < 3; i++) { + accel_corr[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); + } + } +} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h new file mode 100644 index 000000000..3692da77e --- /dev/null +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -0,0 +1,15 @@ +/* + * acceleration_transform.h + * + * Created on: 30.03.2013 + * Author: ton + */ + +#ifndef ACCELERATION_TRANSFORM_H_ +#define ACCELERATION_TRANSFORM_H_ + +#include + +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); + +#endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 4277fe19b..1c6d21ded 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,7 @@ #include "sounds.h" #include #include "kalman_filter_inertial.h" +#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -147,9 +148,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + /* kalman filter K for simulation parameters: + * rate_accel = 200 Hz + * rate_baro = 100 Hz + * err_accel = 1.0 m/s^2 + * err_baro = 0.2 m + */ + static float k[3][2] = { + { 0.0262f, 0.00001f }, + { 0.0349f, 0.00191f }, + { 0.000259f, 0.618f } + }; /* initialize values */ - static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f, - 0.99f } }; static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -160,7 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ - float rho0 = 1.293f; /* standard pressure */ const static float const_earth_gravity = 9.81f; static double lat_current = 0.0d; //[°]] --> 47.0 @@ -203,6 +212,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* FIRST PARAMETER UPDATE */ parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ + + // TODO implement calibration procedure, now put dummy values + int16_t accel_offs[3] = { 0, 0, 0 }; + float accel_T[3][3] = { + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f } + }; + /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -226,12 +244,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static int printcounter = 0; if (printcounter == 100) { printcounter = 0; - printf("[pos_est1D] wait for GPS fix type 3\n"); + printf("[position_estimator_inav] wait for GPS fix type 3\n"); } printcounter++; } - /* get gps value for first initialization */ + /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; @@ -240,20 +258,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", + printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + hrt_abstime last_time = 0; thread_running = true; + static int printatt_counter = 0; + uint32_t accelerometer_counter = 0; + uint32_t baro_counter = 0; + uint16_t accelerometer_updates = 0; + uint16_t baro_updates = 0; + hrt_abstime updates_counter_start = hrt_absolute_time(); + uint32_t updates_counter_len = 1000000; + hrt_abstime pub_last = 0; + uint32_t pub_interval = 5000; // limit publish rate to 200 Hz - /** main loop */ - struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } }; + /* main loop */ + struct pollfd fds[3] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; printf("[position_estimator_inav] main loop started\n"); - static int printatt_counter = 0; while (!thread_should_exit) { - int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate + bool accelerometer_updated = false; + bool baro_updated = false; + int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - } else { + printf("[position_estimator_inav] subscriptions poll error\n", ret); + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -262,7 +298,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &update); /* update parameters */ parameters_update(&pos_inav_param_handles, &pos_inav_params); - //r = pos_inav_params.r; } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -270,16 +305,41 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &vehicle_status); } /* vehicle attitude */ - //if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - //} - /* sensor combined */ if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + } + /* sensor combined */ + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (sensor.accelerometer_counter > accelerometer_counter) { + accelerometer_updated = true; + accelerometer_counter = sensor.accelerometer_counter; + accelerometer_updates++; + } + if (sensor.baro_counter > baro_counter) { + baro_updated = true; + baro_counter = sensor.baro_counter; + baro_updates++; + } + // barometric pressure estimation at start up + if (!local_flag_baroINITdone && baro_updated) { + // mean calculation over several measurements + if (baro_loop_cnt < baro_loop_end) { + baro_alt0 += sensor.baro_alt_meter; + baro_loop_cnt++; + } else { + baro_alt0 /= (float) (baro_loop_cnt); + local_flag_baroINITdone = true; + char str[80]; + sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0); + printf("%s\n", str); + mavlink_log_info(mavlink_fd, str); + } + } } if (use_gps) { /* vehicle GPS position */ - //if (fds[4].revents & POLLIN) { + if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -289,75 +349,65 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); - //} - } - // barometric pressure estimation at start up - if (!local_flag_baroINITdone) { - // mean calculation over several measurements - if (baro_loop_cnt < baro_loop_end) { - baro_alt0 += sensor.baro_alt_meter; - baro_loop_cnt++; - } else { - baro_alt0 /= (float) (baro_loop_cnt); - local_flag_baroINITdone = true; - char *baro_m_start = "barometer initialized with alt0 = "; - char p0_char[15]; - sprintf(p0_char, "%8.2f", baro_alt0 / 100); - char *baro_m_end = " m"; - char str[80]; - strcpy(str, baro_m_start); - strcat(str, p0_char); - strcat(str, baro_m_end); - mavlink_log_info(mavlink_fd, str); } } - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; - /* convert accelerations from UAV frame to NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, 0.0f }; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; - } - } - accel_NED[2] += const_earth_gravity; - /* prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // pos, accel - bool use_z[2] = { false, true }; - if (local_flag_baroINITdone) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + } /* end of poll return value check */ + + hrt_abstime t = hrt_absolute_time(); + float dt = (t - last_time) / 1000000.0; + last_time = t; + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * accel_corr[j]; } + } + accel_NED[2] += const_earth_gravity; + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { /* correction */ kalman_filter_inertial_update(z_est, z_meas, k, use_z); - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[pos_est_inav] dt = %.6f\n", dt); - printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - /* - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[0][0], att.R[0][1], att.R[0][2]); - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[1][0], att.R[1][1], att.R[1][2]); - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[2][0], att.R[2][1], att.R[2][2]); - */ - printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); - } - printatt_counter++; + } + if (t - updates_counter_start > updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + } + if (printatt_counter == 100) { + printatt_counter = 0; + printf("[position_estimator_inav] dt = %.6f\n", dt); + printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", + att.pitch, att.roll, att.yaw); + printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", + sensor.accelerometer_m_s2[0], + sensor.accelerometer_m_s2[1], + sensor.accelerometer_m_s2[2]); + printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", + accel_NED[0], accel_NED[1], accel_NED[2]); + printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", + z_est[0], z_est[1], z_est[2]); + } + printatt_counter++; + if (t - pub_last > pub_interval) { + pub_last = t; local_pos_est.x = 0.0; local_pos_est.vx = 0.0; local_pos_est.y = 0.0; @@ -374,11 +424,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { vehicle_local_position), local_pos_est_pub, &local_pos_est); } - } /* end of poll return value check */ + } } - printf("[pos_est_inav] exiting.\n"); - mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting"); + printf("[position_estimator_inav] exiting.\n"); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; } -- cgit v1.2.3 From 114685a40bf38dc4281224ea18f898b6159df037 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 21:30:58 +0400 Subject: position_estimator_inav - first working version --- apps/position_estimator_inav/Makefile | 3 +- .../acceleration_transform.c | 4 +- .../acceleration_transform.h | 4 +- .../kalman_filter_inertial.c | 14 ++-- .../kalman_filter_inertial.h | 7 ++ .../position_estimator_inav_main.c | 82 +++++++--------------- .../position_estimator_inav_params.c | 69 ++++++++++++++++-- .../position_estimator_inav_params.h | 31 ++++++-- apps/position_estimator_inav/sounds.c | 40 ----------- apps/position_estimator_inav/sounds.h | 16 ----- 10 files changed, 135 insertions(+), 135 deletions(-) delete mode 100644 apps/position_estimator_inav/sounds.c delete mode 100644 apps/position_estimator_inav/sounds.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index e5f3b4b42..192ec1825 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,7 +42,6 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ - acceleration_transform.c \ - sounds.c + acceleration_transform.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 9080c4e20..bd933cab4 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -1,8 +1,8 @@ /* * acceleration_transform.c * - * Created on: 30.03.2013 - * Author: Anton Babushkin + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Transform acceleration vector to true orientation and scale * diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 3692da77e..26bf0d68f 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -1,8 +1,8 @@ /* * acceleration_transform.h * - * Created on: 30.03.2013 - * Author: ton + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin */ #ifndef ACCELERATION_TRANSFORM_H_ diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c index 7de06cb44..64031ee7b 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.c +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -1,3 +1,10 @@ +/* + * kalman_filter_inertial.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + #include "kalman_filter_inertial.h" void kalman_filter_inertial_predict(float dt, float x[3]) { @@ -7,10 +14,9 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; - // y = z - x - for (int i = 0; i < 2; i++) { - y[i] = z[i] - x[i]; - } + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[2]; // x = x + K * y for (int i = 0; i < 3; i++) { // Row for (int j = 0; j < 2; j++) { // Column diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h index e6bbf1315..3e5134c33 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.h +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -1,3 +1,10 @@ +/* + * kalman_filter_inertial.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + #include void kalman_filter_inertial_predict(float dt, float x[3]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 1c6d21ded..b8ef1f76a 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,15 +65,13 @@ #include #include "position_estimator_inav_params.h" -//#include -#include "sounds.h" -#include #include "kalman_filter_inertial.h" #include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -89,7 +85,7 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -106,12 +102,14 @@ int position_estimator_inav_main(int argc, char *argv[]) { usage("missing command"); if (!strcmp(argv[1], "start")) { - if (thread_running) { printf("position_estimator_inav already running\n"); /* this is not an error */ exit(0); } + if (argc > 1) + if (!strcmp(argv[2], "-v")) + verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", @@ -148,17 +146,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); - /* kalman filter K for simulation parameters: - * rate_accel = 200 Hz - * rate_baro = 100 Hz - * err_accel = 1.0 m/s^2 - * err_baro = 0.2 m - */ - static float k[3][2] = { - { 0.0262f, 0.00001f }, - { 0.0349f, 0.00191f }, - { 0.000259f, 0.618f } - }; /* initialize values */ static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -213,14 +200,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ - // TODO implement calibration procedure, now put dummy values - int16_t accel_offs[3] = { 0, 0, 0 }; - float accel_T[3][3] = { - { 1.0f, 0.0f, 0.0f }, - { 0.0f, 1.0f, 0.0f }, - { 0.0f, 0.0f, 1.0f } - }; - /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -270,11 +249,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint16_t baro_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = 0; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[3] = { + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -288,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n", ret); + if (verbose_mode) + printf("[position_estimator_inav] subscriptions poll error\n"); } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -359,7 +339,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -384,34 +364,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, k, use_z); - } - if (t - updates_counter_start > updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); - updates_counter_start = t; + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); } - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[position_estimator_inav] dt = %.6f\n", dt); - printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); + if (verbose_mode) { + /* print updates rate */ + if (t - updates_counter_start > updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + accelerometer_updates = 0; + baro_updates = 0; + } } - printatt_counter++; if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0; - local_pos_est.vx = 0.0; - local_pos_est.y = 0.0; - local_pos_est.vy = 0.0; + local_pos_est.x = 0.0f; + local_pos_est.vx = 0.0f; + local_pos_est.y = 0.0f; + local_pos_est.vy = 0.0f; local_pos_est.z = z_est[0]; local_pos_est.vz = z_est[1]; local_pos_est.timestamp = hrt_absolute_time(); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 5567fd2cf..fb082fbcb 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli -* Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,16 +42,73 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); + +PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); + +PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->r = param_find("POS_EST_R"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); + h->k_alt_01 = param_find("INAV_K_ALT_01"); + h->k_alt_10 = param_find("INAV_K_ALT_10"); + h->k_alt_11 = param_find("INAV_K_ALT_11"); + h->k_alt_20 = param_find("INAV_K_ALT_20"); + h->k_alt_21 = param_find("INAV_K_ALT_21"); + + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); + h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); + h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); + + h->acc_t_00 = param_find("INAV_ACC_T_00"); + h->acc_t_01 = param_find("INAV_ACC_T_01"); + h->acc_t_02 = param_find("INAV_ACC_T_02"); + h->acc_t_10 = param_find("INAV_ACC_T_10"); + h->acc_t_11 = param_find("INAV_ACC_T_11"); + h->acc_t_12 = param_find("INAV_ACC_T_12"); + h->acc_t_20 = param_find("INAV_ACC_T_20"); + h->acc_t_21 = param_find("INAV_ACC_T_21"); + h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->r, &(p->r)); + param_get(h->k_alt_00, &(p->k[0][0])); + param_get(h->k_alt_01, &(p->k[0][1])); + param_get(h->k_alt_10, &(p->k[1][0])); + param_get(h->k_alt_11, &(p->k[1][1])); + param_get(h->k_alt_20, &(p->k[2][0])); + param_get(h->k_alt_21, &(p->k[2][1])); + + param_get(h->acc_offs_0, &(p->acc_offs[0])); + param_get(h->acc_offs_1, &(p->acc_offs[1])); + param_get(h->acc_offs_2, &(p->acc_offs[2])); + + param_get(h->acc_t_00, &(p->acc_T[0][0])); + param_get(h->acc_t_01, &(p->acc_T[0][1])); + param_get(h->acc_t_02, &(p->acc_T[0][2])); + param_get(h->acc_t_10, &(p->acc_T[1][0])); + param_get(h->acc_t_11, &(p->acc_T[1][1])); + param_get(h->acc_t_12, &(p->acc_T[1][2])); + param_get(h->acc_t_20, &(p->acc_T[2][0])); + param_get(h->acc_t_21, &(p->acc_T[2][1])); + param_get(h->acc_t_22, &(p->acc_T[2][2])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index c45ca8135..694bf015b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,11 +41,32 @@ #include struct position_estimator_inav_params { - float r; + float k[3][2]; + int16_t acc_offs[3]; + float acc_T[3][3]; }; struct position_estimator_inav_param_handles { - param_t r; + param_t k_alt_00; + param_t k_alt_01; + param_t k_alt_10; + param_t k_alt_11; + param_t k_alt_20; + param_t k_alt_21; + + param_t acc_offs_0; + param_t acc_offs_1; + param_t acc_offs_2; + + param_t acc_t_00; + param_t acc_t_01; + param_t acc_t_02; + param_t acc_t_10; + param_t acc_t_11; + param_t acc_t_12; + param_t acc_t_20; + param_t acc_t_21; + param_t acc_t_22; }; /** diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c deleted file mode 100644 index 4f7b05fea..000000000 --- a/apps/position_estimator_inav/sounds.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * sounds.c - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#include -#include -#include - - -static int buzzer; - -int sounds_init(void) -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void sounds_deinit(void) -{ - close(buzzer); -} - -void tune_tetris(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 6); -} - -void tune_sonar(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 7); -} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h deleted file mode 100644 index 356e42169..000000000 --- a/apps/position_estimator_inav/sounds.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * sounds.h - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#ifndef SOUNDS_H_ -#define SOUNDS_H_ - -int sounds_init(void); -void sounds_deinit(void); -void tune_tetris(void); -void tune_sonar(void); - -#endif /* SOUNDS_H_ */ -- cgit v1.2.3 From 1be74a4716444e08c47ad3eda1f56db6f2893615 Mon Sep 17 00:00:00 2001 From: Anton Date: Sun, 31 Mar 2013 12:24:37 +0400 Subject: Accelerometers offsets calibration implemented --- .../acceleration_transform.c | 84 ++++++----- .../acceleration_transform.h | 3 + .../position_estimator_inav_main.c | 158 +++++++++++++++++---- 3 files changed, 182 insertions(+), 63 deletions(-) diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index bd933cab4..812920878 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -17,23 +17,23 @@ * * * * * Calibration * * * * - * Tests - * - * accel_corr_x_p = [ g 0 0 ]^T // positive x - * accel_corr_x_n = [ -g 0 0 ]^T // negative x - * accel_corr_y_p = [ 0 g 0 ]^T // positive y - * accel_corr_y_n = [ 0 -g 0 ]^T // negative y - * accel_corr_z_p = [ 0 0 g ]^T // positive z - * accel_corr_z_n = [ 0 0 -g ]^T // negative z - * - * 6 tests * 3 axes = 18 equations + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // positive x + * | -g 0 0 | // negative x + * | 0 g 0 | // positive y + * | 0 -g 0 | // negative y + * | 0 0 g | // positive z + * [ 0 0 -g ] // negative z + * accel_raw_ref[6][3] + * + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 + * + * 6 reference vectors * 3 axes = 18 equations * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants * * Find accel_offs * - * accel_offs = (accel_raw_x_p + accel_raw_x_n + - * accel_raw_y_p + accel_raw_y_n + - * accel_raw_z_p + accel_raw_z_n) / 6 + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 * * * Find accel_T @@ -41,9 +41,8 @@ * 9 unknown constants * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations * - * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + - * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + - * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * * A x = b * * x = [ accel_T[0][0] ] @@ -56,34 +55,35 @@ * | accel_T[2][1] | * [ accel_T[2][2] ] * - * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough - * | accel_corr_x_p[1] | - * | accel_corr_x_p[2] | - * | accel_corr_y_p[0] | - * | accel_corr_y_p[1] | - * | accel_corr_y_p[2] | - * | accel_corr_z_p[0] | - * | accel_corr_z_p[1] | - * [ accel_corr_z_p[2] ] - * - * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis - * ... - * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] - * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | - * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | - * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | - * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | - * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | - * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | - * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | - * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ] + * b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough + * | accel_corr_ref[0][1] | + * | accel_corr_ref[0][2] | + * | accel_corr_ref[2][0] | + * | accel_corr_ref[2][1] | + * | accel_corr_ref[2][2] | + * | accel_corr_ref[4][0] | + * | accel_corr_ref[4][1] | + * [ accel_corr_ref[4][2] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2 + * + * A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ] + * | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 | + * | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] | + * | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 | + * | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 | + * | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] | + * | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 | + * | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 | + * [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ] * * x = A^-1 b */ #include "acceleration_transform.h" -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) { +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]) { for (int i = 0; i < 3; i++) { accel_corr[i] = 0.0f; for (int j = 0; j < 3; j++) { @@ -91,3 +91,11 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel } } } + +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g) { + for (int i = 0; i < 3; i++) { + accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); + } +} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 26bf0d68f..e21aebe73 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -12,4 +12,7 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g); + #endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index b8ef1f76a..552046568 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -72,6 +72,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -84,19 +85,19 @@ static void usage(const char *reason); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); -} + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + exit(1); + } -/** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ + /** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -132,10 +133,112 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } + if (!strcmp(argv[1], "calibrate")) { + do_accelerometer_calibration(); + exit(0); + } + usage("unrecognized command"); exit(1); } +void wait_for_input() { + printf( + "press any key to continue, 'Q' to abort\n"); + while (true ) { + int c = getchar(); + if (c == 'q' || c == 'Q') { + exit(0); + } else { + return; + } + } +} + +void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], + int samples) { + printf("[position_estimator_inav] measuring...\n"); + struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + int count = 0; + int32_t accel_sum[3] = { 0, 0, 0 }; + while (count < samples) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + accel_sum[0] += sensor.accelerometer_raw[0]; + accel_sum[1] += sensor.accelerometer_raw[1]; + accel_sum[2] += sensor.accelerometer_raw[2]; + count++; + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + printf("[position_estimator_inav] no accelerometer data for 1s\n"); + exit(1); + } else { + printf("[position_estimator_inav] poll error\n"); + exit(1); + } + } + for (int i = 0; i < 3; i++) + accel_avg[i] = (accel_sum[i] + count / 2) / count; + printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]); +} + +void do_accelerometer_calibration() { + printf("[position_estimator_inav] calibration started\n"); + const int calibration_samples = 1000; + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int16_t accel_raw_ref[6][3]; // Reference measurements + printf("[position_estimator_inav] place vehicle level, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on it's back, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on right side, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on left side, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle nose down, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle nose up, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), + calibration_samples); + close(sensor_combined_sub); + printf("[position_estimator_inav] reference data collection done\n"); + + int16_t accel_offs[3]; + float accel_T[3][3]; + calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, + const_earth_gravity); + printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", + accel_offs[0], accel_offs[1], accel_offs[2]); + int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] }; + + if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) + || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) + || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) { + printf("[position_estimator_inav] setting parameters failed\n"); + exit(1); + } + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + if (save_ret != 0) { + printf("[position_estimator_inav] auto-save of parameters to storage failed\n"); + exit(1); + } + printf("[position_estimator_inav] calibration done\n"); +} + /**************************************************************************** * main ****************************************************************************/ @@ -157,7 +260,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ - const static float const_earth_gravity = 9.81f; static double lat_current = 0.0d; //[°]] --> 47.0 static double lon_current = 0.0d; //[°]] -->8.5 @@ -237,7 +339,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", + printf( + "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); hrt_abstime last_time = 0; @@ -253,13 +356,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } - }; + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { + .fd = vehicle_status_sub, .events = POLLIN }, { .fd = + vehicle_attitude_sub, .events = POLLIN }, { .fd = + sensor_combined_sub, .events = POLLIN }, { .fd = + vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; @@ -311,7 +412,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { baro_alt0 /= (float) (baro_loop_cnt); local_flag_baroINITdone = true; char str[80]; - sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0); + sprintf(str, + "[position_estimator_inav] baro_alt0 = %.2f", + baro_alt0); printf("%s\n", str); mavlink_log_info(mavlink_fd, str); } @@ -339,7 +442,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, + pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -364,13 +468,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + use_z); } if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + printf( + "[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", + accelerometer_updates / updates_dt, + baro_updates / updates_dt); updates_counter_start = t; accelerometer_updates = 0; baro_updates = 0; -- cgit v1.2.3 From d536e3d5f9b0fc3187cdcd8fc20bb8bb600b9e98 Mon Sep 17 00:00:00 2001 From: Anton Date: Sun, 31 Mar 2013 20:42:15 +0400 Subject: Complete calibration implemented --- .../acceleration_transform.c | 93 ++++++++++++++-------- .../acceleration_transform.h | 5 +- .../position_estimator_inav_main.c | 89 +++++++++++++-------- 3 files changed, 122 insertions(+), 65 deletions(-) diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 812920878..3aba9b403 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -43,41 +43,31 @@ * * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * * A x = b * - * x = [ accel_T[0][0] ] - * | accel_T[0][1] | - * | accel_T[0][2] | - * | accel_T[1][0] | - * | accel_T[1][1] | - * | accel_T[1][2] | - * | accel_T[2][0] | - * | accel_T[2][1] | - * [ accel_T[2][2] ] - * - * b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough - * | accel_corr_ref[0][1] | - * | accel_corr_ref[0][2] | - * | accel_corr_ref[2][0] | - * | accel_corr_ref[2][1] | - * | accel_corr_ref[2][2] | - * | accel_corr_ref[4][0] | - * | accel_corr_ref[4][1] | - * [ accel_corr_ref[4][2] ] - * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2 - * - * A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ] - * | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 | - * | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] | - * | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 | - * | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 | - * | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] | - * | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 | - * | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 | - * [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ] + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] + * + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 + * + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] * * x = A^-1 b + * + * accel_T = A^-1 * g + * g = 9.81 */ #include "acceleration_transform.h" @@ -92,10 +82,49 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], } } -void calculate_calibration_values(int16_t accel_raw_ref[6][3], +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0) + return -1; // Singular matrix + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + return 0; +} + +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g) { + /* calculate raw offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); } + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); + mat_A[i][j] = a; + } + } + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + mat_invert3(mat_A, mat_A_inv); + for (int i = 0; i < 3; i++) { + /* copy results to accel_T */ + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + return 0; } diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index e21aebe73..4d1299db5 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -10,9 +10,10 @@ #include -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]); -void calculate_calibration_values(int16_t accel_raw_ref[6][3], +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g); #endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 552046568..292cf7f21 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -76,12 +76,15 @@ const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); +void do_accelerometer_calibration(); + int position_estimator_inav_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + /** * Print the correct usage. */ -static void usage(const char *reason); - static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -143,8 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) { } void wait_for_input() { - printf( - "press any key to continue, 'Q' to abort\n"); + printf("press any key to continue, 'Q' to abort\n"); while (true ) { int c = getchar(); if (c == 'q' || c == 'Q') { @@ -179,9 +181,11 @@ void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], exit(1); } } - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_avg[i] = (accel_sum[i] + count / 2) / count; - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]); + } + printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], + accel_avg[1], accel_avg[2]); } void do_accelerometer_calibration() { @@ -191,49 +195,72 @@ void do_accelerometer_calibration() { int16_t accel_raw_ref[6][3]; // Reference measurements printf("[position_estimator_inav] place vehicle level, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on it's back, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on right side, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on left side, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), calibration_samples); printf("[position_estimator_inav] place vehicle nose down, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), calibration_samples); printf("[position_estimator_inav] place vehicle nose up, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), calibration_samples); close(sensor_combined_sub); printf("[position_estimator_inav] reference data collection done\n"); int16_t accel_offs[3]; float accel_T[3][3]; - calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, + int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, const_earth_gravity); - printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", + if (res != 0) { + printf( + "[position_estimator_inav] calibration parameters calculation error\n"); + exit(1); + + } + printf( + "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", accel_offs[0], accel_offs[1], accel_offs[2]); - int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] }; + printf( + "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", + accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], + accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], + accel_T[2][2]); + int32_t accel_offs_int32[3] = + { accel_offs[0], accel_offs[1], accel_offs[2] }; if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) { + || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) + || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) + || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) + || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) + || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) + || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) + || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) + || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) + || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) + || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { printf("[position_estimator_inav] setting parameters failed\n"); exit(1); } /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { - printf("[position_estimator_inav] auto-save of parameters to storage failed\n"); + printf( + "[position_estimator_inav] auto-save of parameters to storage failed\n"); exit(1); } printf("[position_estimator_inav] calibration done\n"); @@ -253,8 +280,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - const static float dT_const_120 = 1.0f / 120.0f; - const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; bool use_gps = false; int baro_loop_cnt = 0; @@ -308,12 +333,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .events = POLLIN } }; while (gps.fix_type < 3) { - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ @@ -345,15 +364,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime last_time = 0; thread_running = true; - static int printatt_counter = 0; uint32_t accelerometer_counter = 0; uint32_t baro_counter = 0; uint16_t accelerometer_updates = 0; uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 5000; // limit publish rate to 200 Hz + uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { @@ -368,8 +388,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - if (verbose_mode) - printf("[position_estimator_inav] subscriptions poll error\n"); + printf("[position_estimator_inav] subscriptions poll error\n"); + thread_should_exit = true; + continue; } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -388,6 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; } /* sensor combined */ if (fds[3].revents & POLLIN) { @@ -432,6 +454,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updates++; } } @@ -476,12 +499,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", accelerometer_updates / updates_dt, - baro_updates / updates_dt); + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; accelerometer_updates = 0; baro_updates = 0; + gps_updates = 0; + attitude_updates = 0; } } if (t - pub_last > pub_interval) { -- cgit v1.2.3 From f82af140aea5ea9365cf383e0102056019c99fec Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 31 Mar 2013 20:56:43 -0700 Subject: Hand-merge F427 patches. --- nuttx/arch/arm/include/stm32/chip.h | 97 +++++++++++ nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h | 11 +- nuttx/arch/arm/src/stm32/Kconfig | 110 +++++++++++- nuttx/arch/arm/src/stm32/chip/stm32_flash.h | 25 ++- nuttx/arch/arm/src/stm32/chip/stm32_i2c.h | 20 +++ nuttx/arch/arm/src/stm32/chip/stm32_pwr.h | 11 +- nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h | 5 + nuttx/arch/arm/src/stm32/chip/stm32_uart.h | 18 ++ nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h | 33 +++- .../arm/src/stm32/chip/stm32f40xxx_memorymap.h | 5 + nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h | 50 +++++- nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h | 50 +++++- .../arch/arm/src/stm32/chip/stm32f40xxx_vectors.h | 20 ++- nuttx/arch/arm/src/stm32/stm32_lowputc.c | 34 ++++ nuttx/arch/arm/src/stm32/stm32_serial.c | 193 +++++++++++++++++++- nuttx/arch/arm/src/stm32/stm32_spi.c | 194 ++++++++++++++++++++- nuttx/arch/arm/src/stm32/stm32_spi.h | 22 ++- nuttx/arch/arm/src/stm32/stm32_uart.h | 70 +++++++- nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c | 36 ++++ nuttx/drivers/serial/Kconfig | 194 +++++++++++++++++++++ 20 files changed, 1172 insertions(+), 26 deletions(-) diff --git a/nuttx/arch/arm/include/stm32/chip.h b/nuttx/arch/arm/include/stm32/chip.h index 14d92ea3d..473885471 100644 --- a/nuttx/arch/arm/include/stm32/chip.h +++ b/nuttx/arch/arm/include/stm32/chip.h @@ -688,6 +688,103 @@ # define STM32_NRNG 1 /* Random number generator (RNG) */ # define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ +#elif defined(CONFIG_ARCH_CHIP_STM32F427I) /* BGA176; LQFP176 1024/2048KiB flash 256KiB SRAM */ +# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */ +# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */ +# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */ +# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */ +# undef CONFIG_STM32_VALUELINE /* STM32F100x */ +# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */ +# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */ +# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */ +# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */ +# define STM32_NFSMC 1 /* FSMC */ +# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */ +# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA + * 32-bit general timers TIM2 and 5 with DMA */ +# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */ +# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */ +# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NSPI 6 /* SPI1-6 */ +# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */ +# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */ +# define STM32_NI2C 3 /* I2C1-3 */ +# define STM32_NCAN 2 /* CAN1-2 */ +# define STM32_NSDIO 1 /* SDIO */ +# define STM32_NUSBOTG 1 /* USB OTG FS/HS */ +# define STM32_NGPIO 139 /* GPIOA-I */ +# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */ +# define STM32_NDAC 2 /* 12-bit DAC1-2 */ +# define STM32_NCRC 1 /* CRC */ +# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */ +# define STM32_NRNG 1 /* Random number generator (RNG) */ +# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ + +#elif defined(CONFIG_ARCH_CHIP_STM32F427Z) /* LQFP144 1024/2048KiB flash 256KiB SRAM */ +# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */ +# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */ +# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */ +# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */ +# undef CONFIG_STM32_VALUELINE /* STM32F100x */ +# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */ +# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */ +# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */ +# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */ +# define STM32_NFSMC 1 /* FSMC */ +# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */ +# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA + * 32-bit general timers TIM2 and 5 with DMA */ +# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */ +# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */ +# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NSPI 6 /* SPI1-6 */ +# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */ +# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */ +# define STM32_NI2C 3 /* I2C1-3 */ +# define STM32_NCAN 2 /* CAN1-2 */ +# define STM32_NSDIO 1 /* SDIO */ +# define STM32_NUSBOTG 1 /* USB OTG FS/HS */ +# define STM32_NGPIO 139 /* GPIOA-I */ +# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */ +# define STM32_NDAC 2 /* 12-bit DAC1-2 */ +# define STM32_NCRC 1 /* CRC */ +# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */ +# define STM32_NRNG 1 /* Random number generator (RNG) */ +# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ + +#elif defined(CONFIG_ARCH_CHIP_STM32F427V) /* LQFP100 1024/2048KiB flash 256KiB SRAM */ +# undef CONFIG_STM32_STM32F10XX /* STM32F10xxx family */ +# undef CONFIG_STM32_LOWDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 16/32 Kbytes */ +# undef CONFIG_STM32_MEDIUMDENSITY /* STM32F100x, STM32F101x, STM32F102x and STM32F103x w/ 64/128 Kbytes */ +# undef CONFIG_STM32_HIGHDENSITY /* STM32F100x, STM32F101x, and STM32F103x w/ 256/512 Kbytes */ +# undef CONFIG_STM32_VALUELINE /* STM32F100x */ +# undef CONFIG_STM32_CONNECTIVITYLINE /* STM32F105x and STM32F107x */ +# undef CONFIG_STM32_STM32F20XX /* STM32F205x and STM32F207x */ +# undef CONFIG_STM32_STM32F30XX /* STM32F30xxx family */ +# define CONFIG_STM32_STM32F40XX 1 /* STM32F405xx, STM32407xx and STM32F427/437 */ +# define STM32_NFSMC 1 /* FSMC */ +# define STM32_NATIM 2 /* Two advanced timers TIM1 and 8 */ +# define STM32_NGTIM 4 /* 16-bit general timers TIM3 and 4 with DMA + * 32-bit general timers TIM2 and 5 with DMA */ +# define STM32_NGTIMNDMA 6 /* 16-bit general timers TIM9-14 without DMA */ +# define STM32_NBTIM 2 /* Two basic timers, TIM6-7 */ +# define STM32_NDMA 2 /* DMA1-2 */ +# define STM32_NSPI 4 /* SPI1-4 */ +# define STM32_NI2S 2 /* I2S1-2 (multiplexed with SPI2-3) */ +# define STM32_NUSART 8 /* USART1-3 and 6, UART 4-5 and 7-8 */ +# define STM32_NI2C 3 /* I2C1-3 */ +# define STM32_NCAN 2 /* CAN1-2 */ +# define STM32_NSDIO 1 /* SDIO */ +# define STM32_NUSBOTG 1 /* USB OTG FS/HS */ +# define STM32_NGPIO 139 /* GPIOA-I */ +# define STM32_NADC 3 /* 12-bit ADC1-3, 24 channels */ +# define STM32_NDAC 2 /* 12-bit DAC1-2 */ +# define STM32_NCRC 1 /* CRC */ +# define STM32_NETHERNET 1 /* 100/100 Ethernet MAC */ +# define STM32_NRNG 1 /* Random number generator (RNG) */ +# define STM32_NDCMI 1 /* Digital camera interface (DCMI) */ + + #else # error "Unsupported STM32 chip" #endif diff --git a/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h b/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h index cd97b9c9d..524568778 100644 --- a/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h +++ b/nuttx/arch/arm/include/stm32/stm32f40xxx_irq.h @@ -153,7 +153,16 @@ #define STM32_IRQ_RNG (STM32_IRQ_INTERRUPTS+80) /* 80: Hash and Rng global interrupt */ #define STM32_IRQ_FPU (STM32_IRQ_INTERRUPTS+81) /* 81: FPU global interrupt */ -#define NR_IRQS (STM32_IRQ_INTERRUPTS+82) +#ifndef CONFIG_STM32_STM32F427 +# define NR_IRQS (STM32_IRQ_INTERRUPTS+87) +#else +# define STM32_IRQ_UART7 (STM32_IRQ_INTERRUPTS+82) /* 82: UART7 interrupt */ +# define STM32_IRQ_UART8 (STM32_IRQ_INTERRUPTS+83) /* 83: UART8 interrupt */ +# define STM32_IRQ_SPI4 (STM32_IRQ_INTERRUPTS+84) /* 84: SPI4 interrupt */ +# define STM32_IRQ_SPI5 (STM32_IRQ_INTERRUPTS+85) /* 85: SPI5 interrupt */ +# define STM32_IRQ_SPI6 (STM32_IRQ_INTERRUPTS+86) /* 86: SPI6 interrupt */ +# define NR_IRQS (STM32_IRQ_INTERRUPTS+87) +#endif /**************************************************************************************************** * Public Types diff --git a/nuttx/arch/arm/src/stm32/Kconfig b/nuttx/arch/arm/src/stm32/Kconfig index 41724be2d..ca6a82867 100644 --- a/nuttx/arch/arm/src/stm32/Kconfig +++ b/nuttx/arch/arm/src/stm32/Kconfig @@ -173,6 +173,24 @@ config ARCH_CHIP_STM32F407IG select ARCH_CORTEXM4 select STM32_STM32F40XX +config ARCH_CHIP_STM32F427V + bool "STM32F427V" + select ARCH_CORTEXM4 + select STM32_STM32F40XX + select STM32_STM32F427 + +config ARCH_CHIP_STM32F427Z + bool "STM32F427Z" + select ARCH_CORTEXM4 + select STM32_STM32F40XX + select STM32_STM32F427 + +config ARCH_CHIP_STM32F427I + bool "STM32F427I" + select ARCH_CORTEXM4 + select STM32_STM32F40XX + select STM32_STM32F427 + endchoice config STM32_STM32F10XX @@ -193,6 +211,10 @@ config STM32_STM32F20XX config STM32_STM32F40XX bool +# This is really 427/437, but we treat the two the same. +config STM32_STM32F427 + bool + config STM32_DFU bool "DFU bootloader" default n @@ -370,6 +392,27 @@ config STM32_SPI3 select SPI select STM32_SPI +config STM32_SPI4 + bool "SPI4" + default n + depends on STM32_STM32F427 + select SPI + select STM32_SPI + +config STM32_SPI5 + bool "SPI5" + default n + depends on STM32_STM32F427 + select SPI + select STM32_SPI + +config STM32_SPI6 + bool "SPI6" + default n + depends on STM32_STM32F427 + select SPI + select STM32_SPI + config STM32_SYSCFG bool "SYSCFG" default y @@ -490,6 +533,20 @@ config STM32_USART6 select ARCH_HAVE_USART6 select STM32_USART +config STM32_UART7 + bool "UART7" + default n + depends on STM32_STM32F427 + select ARCH_HAVE_UART7 + select STM32_USART + +config STM32_UART8 + bool "UART8" + default n + depends on STM32_STM32F427 + select ARCH_HAVE_UART8 + select STM32_USART + config STM32_USB bool "USB Device" default n @@ -698,6 +755,7 @@ endmenu config STM32_FLASH_PREFETCH bool "Enable FLASH Pre-fetch" depends on STM32_STM32F20XX || STM32_STM32F40XX + default y if STM32_STM32F427 default n ---help--- Enable FLASH prefetch and F2 and F4 parts (FLASH pre-fetch is always enabled @@ -1966,9 +2024,59 @@ config USART6_RXDMA ---help--- In high data rate usage, Rx DMA may eliminate Rx overrun errors +config USART7_RS485 + bool "RS-485 on USART7" + default n + depends on STM32_USART7 + ---help--- + Enable RS-485 interface on USART7. Your board config will have to + provide GPIO_USART7_RS485_DIR pin definition. Currently it cannot be + used with USART7_RXDMA. + +config USART7_RS485_DIR_POLARITY + int "USART7 RS-485 DIR pin polarity" + default 1 + range 0 1 + depends on USART7_RS485 + ---help--- + Polarity of DIR pin for RS-485 on USART7. Set to state on DIR pin which + enables TX (0 - low / nTXEN, 1 - high / TXEN). + +config USART7_RXDMA + bool "USART7 Rx DMA" + default n + depends on STM32_STM32F40XX && STM32_DMA2 + ---help--- + In high data rate usage, Rx DMA may eliminate Rx overrun errors + +config USART8_RS485 + bool "RS-485 on USART8" + default n + depends on STM32_USART8 + ---help--- + Enable RS-485 interface on USART8. Your board config will have to + provide GPIO_USART8_RS485_DIR pin definition. Currently it cannot be + used with USART8_RXDMA. + +config USART8_RS485_DIR_POLARITY + int "USART8 RS-485 DIR pin polarity" + default 1 + range 0 1 + depends on USART8_RS485 + ---help--- + Polarity of DIR pin for RS-485 on USART8. Set to state on DIR pin which + enables TX (0 - low / nTXEN, 1 - high / TXEN). + +config USART8_RXDMA + bool "USART8 Rx DMA" + default n + depends on STM32_STM32F40XX && STM32_DMA2 + ---help--- + In high data rate usage, Rx DMA may eliminate Rx overrun errors + config SERIAL_TERMIOS bool "Serial driver TERMIOS supported" - depends on STM32_USART1 || STM32_USART2 || STM32_USART3 || STM32_UART4 || STM32_UART5 || STM32_USART6 + depends on STM32_USART1 || STM32_USART2 || STM32_USART3 || STM32_UART4 || STM32_UART5 || STM32_USART6 || STM32_USART7 || STM32_USART8 default n ---help--- Serial driver supports termios.h interfaces (tcsetattr, tcflush, etc.). diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_flash.h b/nuttx/arch/arm/src/stm32/chip/stm32_flash.h index d6fcecc11..9de83893d 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_flash.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_flash.h @@ -55,6 +55,7 @@ #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define STM32_FLASH_NPAGES 8 # define STM32_FLASH_PAGESIZE (128*1024) + /* XXX this is wrong for 427, and not really right for 40x due to mixed page sizes */ #endif #define STM32_FLASH_SIZE (STM32_FLASH_NPAGES * STM32_FLASH_PAGESIZE) @@ -74,6 +75,9 @@ #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define STM32_FLASH_OPTCR_OFFSET 0x0014 #endif +#if defined(CONFIG_STM32_STM32F427) +# define STM32_FLASH_OPTCR1_OFFSET 0x0018 +#endif /* Register Addresses ***************************************************************/ @@ -88,7 +92,10 @@ # define STM32_FLASH_OBR (STM32_FLASHIF_BASE+STM32_FLASH_OBR_OFFSET) # define STM32_FLASH_WRPR (STM32_FLASHIF_BASE+STM32_FLASH_WRPR_OFFSET) #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) -# define STM32_FLASH_OPTCR (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET) +# define STM32_FLASH_OPTCR (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR_OFFSET) +#endif +#if defined(CONFIG_STM32_STM32F427) +# define STM32_FLASH_OPTCR1 (STM32_FLASHIF_BASE+STM32_FLASH_OPTCR1_OFFSET) #endif /* Register Bitfield Definitions ****************************************************/ @@ -150,10 +157,14 @@ #elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define FLASH_CR_PG (1 << 0) /* Bit 0: Programming */ # define FLASH_CR_SER (1 << 1) /* Bit 1: Sector Erase */ -# define FLASH_CR_MER (1 << 2) /* Bit 2: Mass Erase */ +# define FLASH_CR_MER (1 << 2) /* Bit 2: Mass Erase sectors 0..11 */ # define FLASH_CR_SNB_SHIFT (3) /* Bits 3-6: Sector number */ # define FLASH_CR_SNB_MASK (15 << FLASH_CR_SNB_SHIFT) +#if defined(CONFIG_STM32_STM32F427) +# define FLASH_CR_SNB(n) (((n % 12) << FLASH_CR_SNB_SHIFT) | ((n / 12) << 7)) /* Sector n, n=0..23 */ +#else # define FLASH_CR_SNB(n) ((n) << FLASH_CR_SNB_SHIFT) /* Sector n, n=0..11 */ +#endif # define FLASH_CR_PSIZE_SHIFT (8) /* Bits 8-9: Program size */ # define FLASH_CR_PSIZE_MASK (3 << FLASH_CR_PSIZE_SHIFT) # define FLASH_CR_PSIZE_X8 (0 << FLASH_CR_PSIZE_SHIFT) /* 00 program x8 */ @@ -164,6 +175,9 @@ # define FLASH_CR_ERRIE (1 << 25) /* Bit 25: Error interrupt enable */ # define FLASH_CR_LOCK (1 << 31) /* Bit 31: Lock */ #endif +#if defined(CONFIG_STM32_STM32F427) +# define FLASH_CR_MER1 (1 << 15) /* Bit 15: Mass Erase sectors 12..23 */ +#endif /* Flash Option Control Register (OPTCR) */ @@ -187,5 +201,12 @@ # define FLASH_OPTCR_NWRP_MASK (0xfff << FLASH_OPTCR_NWRP_SHIFT) #endif +/* Flash Option Control Register (OPTCR1) */ + +#if defined(CONFIG_STM32_STM32F427) +# define FLASH_OPTCR1_NWRP_SHIFT (16) /* Bits 16-27: Not write protect (high bank) */ +# define FLASH_OPTCR1_NWRP_MASK (0xfff << FLASH_OPTCR_NWRP_SHIFT) +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_FLASH_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h b/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h index f481245e0..cb2934d10 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_i2c.h @@ -51,6 +51,9 @@ #define STM32_I2C_SR2_OFFSET 0x0018 /* Status register 2 (16-bit) */ #define STM32_I2C_CCR_OFFSET 0x001c /* Clock control register (16-bit) */ #define STM32_I2C_TRISE_OFFSET 0x0020 /* TRISE Register (16-bit) */ +#ifdef CONFIG_STM32_STM32F427 +# define STM32_I2C_FLTR_OFFSET 0x0024 /* FLTR Register (16-bit) */ +#endif /* Register Addresses ***************************************************************/ @@ -64,6 +67,9 @@ # define STM32_I2C1_SR2 (STM32_I2C1_BASE+STM32_I2C_SR2_OFFSET) # define STM32_I2C1_CCR (STM32_I2C1_BASE+STM32_I2C_CCR_OFFSET) # define STM32_I2C1_TRISE (STM32_I2C1_BASE+STM32_I2C_TRISE_OFFSET) +# ifdef STM32_I2C_FLTR_OFFSET +# define STM32_I2C1_FLTR (STM32_I2C1_BASE+STM32_I2C_FLTR_OFFSET) +# endif #endif #if STM32_NI2C > 1 @@ -76,6 +82,9 @@ # define STM32_I2C2_SR2 (STM32_I2C2_BASE+STM32_I2C_SR2_OFFSET) # define STM32_I2C2_CCR (STM32_I2C2_BASE+STM32_I2C_CCR_OFFSET) # define STM32_I2C2_TRISE (STM32_I2C2_BASE+STM32_I2C_TRISE_OFFSET) +# ifdef STM32_I2C_FLTR_OFFSET +# define STM32_I2C2_FLTR (STM32_I2C2_BASE+STM32_I2C_FLTR_OFFSET) +# endif #endif #if STM32_NI2C > 2 @@ -88,6 +97,9 @@ # define STM32_I2C3_SR2 (STM32_I2C3_BASE+STM32_I2C_SR2_OFFSET) # define STM32_I2C3_CCR (STM32_I2C3_BASE+STM32_I2C_CCR_OFFSET) # define STM32_I2C3_TRISE (STM32_I2C3_BASE+STM32_I2C_TRISE_OFFSET) +# ifdef STM32_I2C_FLTR_OFFSET +# define STM32_I2C3_FLTR (STM32_I2C3_BASE+STM32_I2C_FLTR_OFFSET) +# endif #endif /* Register Bitfield Definitions ****************************************************/ @@ -188,5 +200,13 @@ #define I2C_TRISE_SHIFT (0) /* Bits 5-0: Maximum Rise Time in Fast/Standard mode (Master mode) */ #define I2C_TRISE_MASK (0x3f << I2C_TRISE_SHIFT) +/* FLTR Register */ + +#ifdef STM32_I2C_FLTR_OFFSET +# define I2C_FLTR_ANOFF (1 << 4) /* Bit 4: Analog noise filter disable */ +# define I2C_FLTR_DNF_SHIFT 0 /* Bits 0-3: Digital noise filter */ +# define I2C_FLTR_DNF_MASK (0xf << I2C_FLTR_DNF_SHIFT) +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_I2C_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h b/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h index 2ece6a357..72f19b6df 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_pwr.h @@ -80,7 +80,16 @@ #if defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) # define PWR_CR_FPDS (1 << 9) /* Bit 9: Flash power down in Stop mode */ -# define PWR_CR_VOS (1 << 14) /* Bit 14: Regulator voltage scaling output selection */ +# if defined(CONFIG_STM32_STM32F427) +# define PWR_CR_ADCDC1 (1 << 13) /* Bit 13: see AN4073 for details */ +# define PWR_CR_VOS_SCALE_1 (3 << 14) /* Fmax = 168MHz */ +# define PWR_CR_VOS_SCALE_2 (2 << 14) /* Fmax = 144MHz */ +# define PWR_CR_VOS_SCALE_3 (1 << 14) /* Fmax = 120MHz */ +# define PWR_CR_VOS_MASK (3 << 14) /* Bits 14-15: Regulator voltage scaling output selection */ +# else +# define PWR_CR_VOS (1 << 14) /* Bit 14: Regulator voltage scaling output selection */ + /* 0: Fmax = 144MHz 1: Fmax = 168MHz */ +# endif #endif /* Power control/status register */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h index 6642b1305..dca3820fd 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h @@ -89,6 +89,11 @@ /* SYSCFG peripheral mode configuration register */ #define SYSCFG_PMC_MII_RMII_SEL (1 << 23) /* Bit 23: Ethernet PHY interface selection */ ++#ifdef CONFIG_STM32_STM32F427 ++# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */ ++# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */ ++# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */ ++#endif /* SYSCFG external interrupt configuration register 1-4 */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_uart.h b/nuttx/arch/arm/src/stm32/chip/stm32_uart.h index d3c1e137e..38852553d 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_uart.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_uart.h @@ -118,6 +118,24 @@ # define STM32_USART6_GTPR (STM32_USART6_BASE+STM32_USART_GTPR_OFFSET) #endif +#if STM32_NUSART > 6 +# define STM32_UART7_SR (STM32_UART7_BASE+STM32_USART_SR_OFFSET) +# define STM32_UART7_DR (STM32_UART7_BASE+STM32_USART_DR_OFFSET) +# define STM32_UART7_BRR (STM32_UART7_BASE+STM32_USART_BRR_OFFSET) +# define STM32_UART7_CR1 (STM32_UART7_BASE+STM32_USART_CR1_OFFSET) +# define STM32_UART7_CR2 (STM32_UART7_BASE+STM32_USART_CR2_OFFSET) +# define STM32_UART7_CR3 (STM32_UART7_BASE+STM32_USART_CR3_OFFSET) +#endif + +#if STM32_NUSART > 7 +# define STM32_UART8_SR (STM32_UART8_BASE+STM32_USART_SR_OFFSET) +# define STM32_UART8_DR (STM32_UART8_BASE+STM32_USART_DR_OFFSET) +# define STM32_UART8_BRR (STM32_UART8_BASE+STM32_USART_BRR_OFFSET) +# define STM32_UART8_CR1 (STM32_UART8_BASE+STM32_USART_CR1_OFFSET) +# define STM32_UART8_CR2 (STM32_UART8_BASE+STM32_USART_CR2_OFFSET) +# define STM32_UART8_CR3 (STM32_UART8_BASE+STM32_USART_CR3_OFFSET) +#endif + /* Register Bitfield Definitions ****************************************************/ /* Status register */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h index ab0cfceac..ddd0413a5 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_dma.h @@ -438,11 +438,21 @@ #define DMAMAP_USART2_TX STM32_DMA_MAP(DMA1,DMA_STREAM6,DMA_CHAN4) #define DMAMAP_UART5_TX STM32_DMA_MAP(DMA1,DMA_STREAM7,DMA_CHAN4) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_UART8_TX STM32_DMA_MAP(CMA1,DMA_STREAM0,DMA_CHAN5) +# define DMAMAP_UART7_TX STM32_DMA_MAP(CMA1,DMA_STREAM1,DMA_CHAN5) +#endif #define DMAMAP_TIM3_CH4 STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN5) #define DMAMAP_TIM3_UP STM32_DMA_MAP(DMA1,DMA_STREAM2,DMA_CHAN5) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_UART7_RX STM32_DMA_MAP(CMA1,DMA_STREAM3,DMA_CHAN5) +#endif #define DMAMAP_TIM3_CH1 STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN5) #define DMAMAP_TIM3_TRIG STM32_DMA_MAP(DMA1,DMA_STREAM4,DMA_CHAN5) #define DMAMAP_TIM3_CH2 STM32_DMA_MAP(DMA1,DMA_STREAM5,DMA_CHAN5) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_UART8_RX STM32_DMA_MAP(CMA1,DMA_STREAM6,DMA_CHAN5) +#endif #define DMAMAP_TIM3_CH3 STM32_DMA_MAP(DMA1,DMA_STREAM7,DMA_CHAN5) #define DMAMAP_TIM5_CH3 STM32_DMA_MAP(DMA1,DMA_STREAM0,DMA_CHAN6) @@ -475,10 +485,18 @@ #define DMAMAP_DCMI_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN1) #define DMAMAP_ADC2_1 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN1) #define DMAMAP_ADC2_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN1) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI6_TX STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN1) +# define DMAMAP_SPI6_RX STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN1) +#endif #define DMAMAP_DCMI_2 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN1) #define DMAMAP_ADC3_1 STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN2) #define DMAMAP_ADC3_2 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN2) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI5_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN2) +# define DMAMAP_SPI5_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN2) +#endif #define DMAMAP_CRYP_OUT STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN2) #define DMAMAP_CRYP_IN STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN2) #define DMAMAP_HASH_IN STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN2) @@ -488,6 +506,11 @@ #define DMAMAP_SPI1_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN3) #define DMAMAP_SPI1_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN3) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI4_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM0,DMA_CHAN4) +# define DMAMAP_SPI4_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN4) +#endif +#define DMAMAP_SPI4_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN4) #define DMAMAP_USART1_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN4) #define DMAMAP_SDIO_1 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN4) #define DMAMAP_USART1_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN4) @@ -496,6 +519,10 @@ #define DMAMAP_USART6_RX_1 STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN5) #define DMAMAP_USART6_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN5) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI4_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN5) +# define DMAMAP_SPI4_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN5) +#endif #define DMAMAP_USART6_TX_1 STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN5) #define DMAMAP_USART6_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN5) @@ -512,7 +539,11 @@ #define DMAMAP_TIM8_UP STM32_DMA_MAP(DMA2,DMA_STREAM1,DMA_CHAN7) #define DMAMAP_TIM8_CH1_2 STM32_DMA_MAP(DMA2,DMA_STREAM2,DMA_CHAN7) #define DMAMAP_TIM8_CH2_2 STM32_DMA_MAP(DMA2,DMA_STREAM3,DMA_CHAN7) -#define DMAMAP_TIM8_CH3_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN7) +#define DMAMAP_TIM8_CH3_2 STM32_DMA_MAP(DMA2,DMA_STREAM4,DMA_CHAN7) +#ifdef CONFIG_STM32_STM32F427 +# define DMAMAP_SPI5_RX_2 STM32_DMA_MAP(DMA2,DMA_STREAM5,DMA_CHAN7) +# define DMAMAP_SPI5_TX_2 STM32_DMA_MAP(DMA2,DMA_STREAM6,DMA_CHAN7) +#endif #define DMAMAP_TIM8_CH4 STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7) #define DMAMAP_TIM8_TRIG STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7) #define DMAMAP_TIM8_COM STM32_DMA_MAP(DMA2,DMA_STREAM7,DMA_CHAN7) diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h index 6b9912121..6dc9530fb 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_memorymap.h @@ -140,6 +140,8 @@ #define STM32_CAN2_BASE 0x40006800 /* 0x40006800-0x40006bff: bxCAN2 */ #define STM32_PWR_BASE 0x40007000 /* 0x40007000-0x400073ff: Power control PWR */ #define STM32_DAC_BASE 0x40007400 /* 0x40007400-0x400077ff: DAC */ +#define STM32_UART7_BASE 0x40007800 /* 0x40007800-0x40007bff: UART7 */ +#define STM32_UART8_BASE 0x40007c00 /* 0x40007c00-0x40007fff: UART8 */ /* APB2 Base Addresses **************************************************************/ @@ -154,11 +156,14 @@ # define STM32_ADCCMN_BASE 0x40012300 /* Common */ #define STM32_SDIO_BASE 0x40012c00 /* 0x40012c00-0x40012fff: SDIO */ #define STM32_SPI1_BASE 0x40013000 /* 0x40013000-0x400133ff: SPI1 */ +#define STM32_SPI4_BASE 0x40013400 /* 0x40013000-0x400137ff: SPI4 */ #define STM32_SYSCFG_BASE 0x40013800 /* 0x40013800-0x40013bff: SYSCFG */ #define STM32_EXTI_BASE 0x40013c00 /* 0x40013c00-0x40013fff: EXTI */ #define STM32_TIM9_BASE 0x40014000 /* 0x40014000-0x400143ff: TIM9 timer */ #define STM32_TIM10_BASE 0x40014400 /* 0x40014400-0x400147ff: TIM10 timer */ #define STM32_TIM11_BASE 0x40014800 /* 0x40014800-0x40014bff: TIM11 timer */ +#define STM32_SPI5_BASE 0x40015000 /* 0x40015000-0x400153ff: SPI5 */ +#define STM32_SPI6_BASE 0x40015400 /* 0x40015400-0x400157ff: SPI6 */ /* AHB1 Base Addresses **************************************************************/ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h index 31e51caf0..ed3f09c01 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_pinmap.h @@ -331,6 +331,9 @@ #define GPIO_I2S2_CK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN10) #define GPIO_I2S2_CK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN13) #define GPIO_I2S2_CK_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN1) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_I2S2_CK_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTD|GPIO_PIN3) +#endif #define GPIO_I2S2_MCK (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN6) #define GPIO_I2S2_SD_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN15) #define GPIO_I2S2_SD_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN3) @@ -339,6 +342,9 @@ #define GPIO_I2S2_WS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN6) #define GPIO_I2S2_WS_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN9) #define GPIO_I2S2_WS_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN0) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_I2S2_WS_6 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN6) +#endif #define GPIO_I2S2EXT_SD_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN14) #define GPIO_I2S2EXT_SD_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN2) @@ -349,6 +355,9 @@ #define GPIO_I2S3_MCK (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN7) #define GPIO_I2S3_SD_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN5) #define GPIO_I2S3_SD_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN12) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_I2S3_SD_3 (GPIO_ALT|GPIO_AF6|GPIO_PORTD|GPIO_PIN6) +#endif #define GPIO_I2S3_WS_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN4) #define GPIO_I2S3_WS_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN15) @@ -428,7 +437,7 @@ #define GPIO_SPI2_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN14) #define GPIO_SPI2_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI2_MISO_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN2) -#define GPIO_SPI2_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI2_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN15) #define GPIO_SPI2_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTC|GPIO_PIN3) #define GPIO_SPI2_MOSI_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN3) #define GPIO_SPI2_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN12) @@ -437,16 +446,45 @@ #define GPIO_SPI2_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN10) #define GPIO_SPI2_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTB|GPIO_PIN13) #define GPIO_SPI2_SCK_3 (GPIO_ALT|GPIO_AF5|GPIO_PORTI|GPIO_PIN1) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_SPI2_SCK_4 (GPIO_ALT|GPIO_AF5|GPIO_PORTD|GPIO_PIN3) +#endif #define GPIO_SPI3_MISO_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN4) #define GPIO_SPI3_MISO_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN11) #define GPIO_SPI3_MOSI_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN5) #define GPIO_SPI3_MOSI_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN12) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_SPI3_MOSI_3 (GPIO_ALT|GPIO_AF6|GPIO_PORTD|GPIO_PIN6) +#endif #define GPIO_SPI3_NSS_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN15) #define GPIO_SPI3_NSS_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTA|GPIO_PIN4) #define GPIO_SPI3_SCK_1 (GPIO_ALT|GPIO_AF6|GPIO_PORTB|GPIO_PIN3) #define GPIO_SPI3_SCK_2 (GPIO_ALT|GPIO_AF6|GPIO_PORTC|GPIO_PIN10) +#define GPIO_SPI4_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN5) +#define GPIO_SPI4_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN13) +#define GPIO_SPI4_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN6) +#define GPIO_SPI4_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN14) +#define GPIO_SPI4_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPI4_NSS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN11) +#define GPIO_SPI4_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI4_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTE|GPIO_PIN12) + +#define GPIO_SPI5_MISO_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN8) +#define GPIO_SPI5_MISO_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN7) +#define GPIO_SPI5_MOSI_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN9) +#define GPIO_SPI5_MOSI_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN11) +#define GPIO_SPI5_NSS_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN6) +#define GPIO_SPI5_NSS_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN5) +#define GPIO_SPI5_SCK_1 (GPIO_ALT|GPIO_AF5|GPIO_PORTF|GPIO_PIN7) +#define GPIO_SPI5_SCK_2 (GPIO_ALT|GPIO_AF5|GPIO_PORTH|GPIO_PIN6) + +#define GPIO_SPI6_MISO (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN12) +#define GPIO_SPI6_MOSI (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN14) +#define GPIO_SPI6_NSS (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN8) +#define GPIO_SPI6_SCK (GPIO_ALT|GPIO_AF5|GPIO_PORTG|GPIO_PIN13) + /* Timers */ #define GPIO_TIM1_BKIN_1 (GPIO_ALT|GPIO_AF1|GPIO_PORTA|GPIO_PIN6) @@ -691,5 +729,15 @@ #define GPIO_USART6_TX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN6) #define GPIO_USART6_TX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTG|GPIO_PIN14) +#ifdef CONFIG_STM32_STM32F427 +# define GPIO_UART7_RX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN7) +# define GPIO_UART7_RX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTF|GPIO_PIN6) +# define GPIO_UART7_TX_1 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN8) +# define GPIO_UART7_TX_2 (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTF|GPIO_PIN7) + +# define GPIO_UART8_RX (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN0) +# define GPIO_UART8_TX (GPIO_ALT|GPIO_AF8|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN1) +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32F40XXX_PINMAP_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h index 04cb05741..8ab09c478 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_rcc.h @@ -65,6 +65,9 @@ #define STM32_RCC_CSR_OFFSET 0x0074 /* Control/status register */ #define STM32_RCC_SSCGR_OFFSET 0x0080 /* Spread spectrum clock generation register */ #define STM32_RCC_PLLI2SCFGR_OFFSET 0x0084 /* PLLI2S configuration register */ +#ifdef CONFIG_STM32_STM32F427 +# define STM32_RCC_DCKCFGR_OFFSET 0x008c /* Dedicated clocks configuration register */ +#endif /* Register Addresses *******************************************************************************/ @@ -91,6 +94,9 @@ #define STM32_RCC_CSR (STM32_RCC_BASE+STM32_RCC_CSR_OFFSET) #define STM32_RCC_SSCGR (STM32_RCC_BASE+STM32_RCC_SSCGR_OFFSET) #define STM32_RCC_PLLI2SCFGR (STM32_RCC_BASE+STM32_RCC_PLLI2SCFGR_OFFSET) +#ifdef CONFIG_STM32_STM32F427 +# define STM32_RCC_DCKCFGR (STM32_RCC_BASE+STM32_RCC_DCKCFGR_OFFSET) +#endif /* Register Bitfield Definitions ********************************************************************/ @@ -282,6 +288,10 @@ #define RCC_APB1RSTR_CAN2RST (1 << 26) /* Bit 26: CAN2 reset */ #define RCC_APB1RSTR_PWRRST (1 << 28) /* Bit 28: Power interface reset */ #define RCC_APB1RSTR_DACRST (1 << 29) /* Bit 29: DAC reset */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB1RSTR_UART7RST (1 << 30) /* Bit 30: USART 7 reset */ +# define RCC_APB1RSTR_UART8RST (1 << 31) /* Bit 31: USART 8 reset */ +#endif /* APB2 Peripheral reset register */ @@ -291,11 +301,18 @@ #define RCC_APB2RSTR_USART6RST (1 << 5) /* Bit 5: USART6 reset */ #define RCC_APB2RSTR_ADCRST (1 << 8) /* Bit 8: ADC interface reset (common to all ADCs) */ #define RCC_APB2RSTR_SDIORST (1 << 11) /* Bit 11: SDIO reset */ -#define RCC_APB2RSTR_SPI1RST (1 << 12) /* Bit 12: SPI 1 reset */ +#define RCC_APB2RSTR_SPI1RST (1 << 12) /* Bit 12: SPI1 reset */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2RSTR_SPI4RST (1 << 13) /* Bit 13: SPI4 reset */ +#endif #define RCC_APB2RSTR_SYSCFGRST (1 << 14) /* Bit 14: System configuration controller reset */ #define RCC_APB2RSTR_TIM9RST (1 << 16) /* Bit 16: TIM9 reset */ #define RCC_APB2RSTR_TIM10RST (1 << 17) /* Bit 17: TIM10 reset */ #define RCC_APB2RSTR_TIM11RST (1 << 18) /* Bit 18: TIM11 reset */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2RSTR_SPI5RST (1 << 20) /* Bit 20: SPI 5 reset */ +# define RCC_APB2RSTR_SPI6RST (1 << 21) /* Bit 21: SPI 6 reset */ +#endif /* AHB1 Peripheral Clock enable register */ @@ -358,6 +375,10 @@ #define RCC_APB1ENR_CAN2EN (1 << 26) /* Bit 26: CAN 2 clock enable */ #define RCC_APB1ENR_PWREN (1 << 28) /* Bit 28: Power interface clock enable */ #define RCC_APB1ENR_DACEN (1 << 29) /* Bit 29: DAC interface clock enable */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB1ENR_UART7EN (1 << 30) /* Bit 30: UART7 clock enable */ +# define RCC_APB1ENR_UART8EN (1 << 31) /* Bit 31: UART8 clock enable */ +#endif /* APB2 Peripheral Clock enable register */ @@ -370,10 +391,17 @@ #define RCC_APB2ENR_ADC3EN (1 << 10) /* Bit 10: ADC3 clock enable */ #define RCC_APB2ENR_SDIOEN (1 << 11) /* Bit 11: SDIO clock enable */ #define RCC_APB2ENR_SPI1EN (1 << 12) /* Bit 12: SPI1 clock enable */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2ENR_SPI4EN (1 << 13) /* Bit 13: SPI4 clock enable */ +#endif #define RCC_APB2ENR_SYSCFGEN (1 << 14) /* Bit 14: System configuration controller clock enable */ #define RCC_APB2ENR_TIM9EN (1 << 16) /* Bit 16: TIM9 clock enable */ #define RCC_APB2ENR_TIM10EN (1 << 17) /* Bit 17: TIM10 clock enable */ #define RCC_APB2ENR_TIM11EN (1 << 18) /* Bit 18: TIM11 clock enable */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2ENR_SPI5EN (1 << 20) /* Bit 20: SPI5 clock enable */ +# define RCC_APB2ENR_SPI6EN (1 << 21) /* Bit 21: SPI6 clock enable */ +#endif /* RCC AHB1 low power modeperipheral clock enable register */ @@ -392,6 +420,9 @@ #define RCC_AHB1LPENR_SRAM1LPEN (1 << 16) /* Bit 16: SRAM 1 interface clock enable during Sleep mode */ #define RCC_AHB1LPENR_SRAM2LPEN (1 << 17) /* Bit 17: SRAM 2 interface clock enable during Sleep mode */ #define RCC_AHB1LPENR_BKPSRAMLPEN (1 << 18) /* Bit 18: Backup SRAM interface clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_AHB1LPENR_SRAM3LPEN (1 << 19) /* Bit 19: SRAM 3 interface clock enable during Sleep mode */ +#endif #define RCC_AHB1LPENR_CCMDATARAMLPEN (1 << 20) /* Bit 20: CCM data RAM clock enable during Sleep mode */ #define RCC_AHB1LPENR_DMA1LPEN (1 << 21) /* Bit 21: DMA1 clock enable during Sleep mode */ #define RCC_AHB1LPENR_DMA2LPEN (1 << 22) /* Bit 22: DMA2 clock enable during Sleep mode */ @@ -440,6 +471,10 @@ #define RCC_APB1LPENR_CAN2LPEN (1 << 26) /* Bit 26: CAN 2 clock enable during Sleep mode */ #define RCC_APB1LPENR_PWRLPEN (1 << 28) /* Bit 28: Power interface clock enable during Sleep mode */ #define RCC_APB1LPENR_DACLPEN (1 << 29) /* Bit 29: DAC interface clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB1LPENR_UART7LPEN (1 << 30) /* Bit 30: UART7 clock enable during Sleep mode */ +# define RCC_APB1LPENR_UART8LPEN (1 << 31) /* Bit 31: UART8 clock enable during Sleep mode */ +#endif /* RCC APB2 low power modeperipheral clock enable register */ @@ -452,10 +487,17 @@ #define RCC_APB2LPENR_ADC3LPEN (1 << 10) /* Bit 10: ADC3 clock enable during Sleep mode */ #define RCC_APB2LPENR_SDIOLPEN (1 << 11) /* Bit 11: SDIO clock enable during Sleep mode */ #define RCC_APB2LPENR_SPI1LPEN (1 << 12) /* Bit 12: SPI1 clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2LPENR_SPI4LPEN (1 << 13) /* Bit 13: SPI4 clock enable during Sleep mode */ +#endif #define RCC_APB2LPENR_SYSCFGLPEN (1 << 14) /* Bit 14: System configuration controller clock enable during Sleep mode */ #define RCC_APB2LPENR_TIM9LPEN (1 << 16) /* Bit 16: TIM9 clock enable during Sleep mode */ #define RCC_APB2LPENR_TIM10LPEN (1 << 17) /* Bit 17: TIM10 clock enable during Sleep mode */ #define RCC_APB2LPENR_TIM11LPEN (1 << 18) /* Bit 18: TIM11 clock enable during Sleep mode */ +#ifdef CONFIG_STM32_STM32F427 +# define RCC_APB2LPENR_SPI5LPEN (1 << 20) /* Bit 20: SPI5 clock enable during Sleep mode */ +# define RCC_APB2LPENR_SPI6LPEN (1 << 21) /* Bit 21: SPI6 clock enable during Sleep mode */ +#endif /* Backup domain control register */ @@ -502,5 +544,11 @@ #define RCC_PLLI2SCFGR_PLLI2SR_SHIFT (28) /* Bits 28-30: PLLI2S division factor for I2S clocks */ #define RCC_PLLI2SCFGR_PLLI2SR_MASK (7 << RCC_PLLI2SCFGR_PLLI2SR_SHIFT) +/* Dedicated clocks configuration register */ + +#ifdef CONFIG_STM32_STM32F427 +# define RCC_DCKCFGR_TIMPRE (1 << 24) /* Bit 24: Timer clock prescaler selection */ +#endif + #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32F40XXX_RCC_H */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h index 8db1bd19e..31453c6a4 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h @@ -40,7 +40,7 @@ /* This file is included by stm32_vectors.S. It provides the macro VECTOR that * supplies ach STM32F40xxx vector in terms of a (lower-case) ISR label and an * (upper-case) IRQ number as defined in arch/arm/include/stm32/stm32f40xxx_irq.h. - * stm32_vectors.S will defined the VECTOR in different ways in order to generate + * stm32_vectors.S will define the VECTOR macro in different ways in order to generate * the interrupt vectors and handlers in their final form. */ @@ -50,9 +50,13 @@ #ifdef CONFIG_ARMV7M_CMNVECTOR -/* Reserve 82 interrupt table entries for I/O interrupts. */ +/* Reserve interrupt table entries for I/O interrupts. */ -# define ARMV7M_PERIPHERAL_INTERRUPTS 82 +# ifdef CONFIG_STM32_STM32F427 +# define ARMV7M_PERIPHERAL_INTERRUPTS 87 +# else +# define ARMV7M_PERIPHERAL_INTERRUPTS 82 +# endif #else @@ -133,10 +137,18 @@ VECTOR(stm32_i2c3er, STM32_IRQ_I2C3ER) /* Vector 16+73: I2C3 error int VECTOR(stm32_otghsep1out, STM32_IRQ_OTGHSEP1OUT) /* Vector 16+74: USB On The Go HS End Point 1 Out global interrupt */ VECTOR(stm32_otghsep1in, STM32_IRQ_OTGHSEP1IN) /* Vector 16+75: USB On The Go HS End Point 1 In global interrupt */ VECTOR(stm32_otghswkup, STM32_IRQ_OTGHSWKUP) /* Vector 16+76: USB On The Go HS Wakeup through EXTI interrupt */ -VECTOR(stm32_otghs, STM32_IRQ_OTGHS ) /* Vector 16+77: USB On The Go HS global interrupt */ +VECTOR(stm32_otghs, STM32_IRQ_OTGHS) /* Vector 16+77: USB On The Go HS global interrupt */ VECTOR(stm32_dcmi, STM32_IRQ_DCMI) /* Vector 16+78: DCMI global interrupt */ VECTOR(stm32_cryp, STM32_IRQ_CRYP) /* Vector 16+79: CRYP crypto global interrupt */ VECTOR(stm32_hash, STM32_IRQ_HASH) /* Vector 16+80: Hash and Rng global interrupt */ VECTOR(stm32_fpu, STM32_IRQ_FPU) /* Vector 16+81: FPU global interrupt */ +#ifdef CONFIG_STM32_STM32F427 +VECTOR(stm32_uart7, STM32_IRQ_UART7) /* Vector 16+82: UART7 interrupt */ +VECTOR(stm32_uart8, STM32_IRQ_UART8) /* Vector 16+83: UART8 interrupt */ +VECTOR(stm32_spi4, STM32_IRQ_SPI4) /* Vector 16+84: SPI4 interrupt */ +VECTOR(stm32_spi5, STM32_IRQ_SPI5) /* Vector 16+85: SPI5 interrupt */ +VECTOR(stm32_spi6, STM32_IRQ_SPI6) /* Vector 16+86: SPI6 interrupt */ +#endif + #endif /* CONFIG_ARMV7M_CMNVECTOR */ diff --git a/nuttx/arch/arm/src/stm32/stm32_lowputc.c b/nuttx/arch/arm/src/stm32/stm32_lowputc.c index 6cb07dad9..5ad0ce7d3 100644 --- a/nuttx/arch/arm/src/stm32/stm32_lowputc.c +++ b/nuttx/arch/arm/src/stm32/stm32_lowputc.c @@ -160,6 +160,40 @@ # define STM32_CONSOLE_RS485_DIR_POLARITY true # endif # endif +#elif defined(CONFIG_UART7_SERIAL_CONSOLE) +# define STM32_CONSOLE_BASE STM32_UART7_BASE +# define STM32_APBCLOCK STM32_PCLK1_FREQUENCY +# define STM32_CONSOLE_BAUD CONFIG_UART7_BAUD +# define STM32_CONSOLE_BITS CONFIG_UART7_BITS +# define STM32_CONSOLE_PARITY CONFIG_UART7_PARITY +# define STM32_CONSOLE_2STOP CONFIG_UART7_2STOP +# define STM32_CONSOLE_TX GPIO_UART7_TX +# define STM32_CONSOLE_RX GPIO_UART7_RX +# ifdef CONFIG_UART7_RS485 +# define STM32_CONSOLE_RS485_DIR GPIO_UART7_RS485_DIR +# if (CONFIG_UART7_RS485_DIR_POLARITY == 0) +# define STM32_CONSOLE_RS485_DIR_POLARITY false +# else +# define STM32_CONSOLE_RS485_DIR_POLARITY true +# endif +# endif +#elif defined(CONFIG_UART8_SERIAL_CONSOLE) +# define STM32_CONSOLE_BASE STM32_UART8_BASE +# define STM32_APBCLOCK STM32_PCLK1_FREQUENCY +# define STM32_CONSOLE_BAUD CONFIG_UART8_BAUD +# define STM32_CONSOLE_BITS CONFIG_UART8_BITS +# define STM32_CONSOLE_PARITY CONFIG_UART8_PARITY +# define STM32_CONSOLE_2STOP CONFIG_UART8_2STOP +# define STM32_CONSOLE_TX GPIO_UART8_TX +# define STM32_CONSOLE_RX GPIO_UART8_RX +# ifdef CONFIG_UART8_RS485 +# define STM32_CONSOLE_RS485_DIR GPIO_UART8_RS485_DIR +# if (CONFIG_UART8_RS485_DIR_POLARITY == 0) +# define STM32_CONSOLE_RS485_DIR_POLARITY false +# else +# define STM32_CONSOLE_RS485_DIR_POLARITY true +# endif +# endif #endif /* CR1 settings */ diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c index c91f6a6d7..3273e52da 100644 --- a/nuttx/arch/arm/src/stm32/stm32_serial.c +++ b/nuttx/arch/arm/src/stm32/stm32_serial.c @@ -90,9 +90,10 @@ # endif # if defined(CONFIG_USART2_RXDMA) || defined(CONFIG_USART3_RXDMA) || \ - defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA) + defined(CONFIG_UART4_RXDMA) || defined(CONFIG_UART5_RXDMA) || \ + defined(CONFIG_UART7_RXDMA) || defined(CONFIG_UART8_RXDMA) # ifndef CONFIG_STM32_DMA1 -# error STM32 USART2/3/4/5 receive DMA requires CONFIG_STM32_DMA1 +# error STM32 USART2/3/4/5/7/8 receive DMA requires CONFIG_STM32_DMA1 # endif # endif @@ -105,7 +106,9 @@ (defined(CONFIG_USART3_RXDMA) && defined(CONFIG_USART3_RS485)) || \ (defined(CONFIG_UART4_RXDMA) && defined(CONFIG_UART4_RS485)) || \ (defined(CONFIG_UART5_RXDMA) && defined(CONFIG_UART5_RS485)) || \ - (defined(CONFIG_USART6_RXDMA) && defined(CONFIG_USART6_RS485)) + (defined(CONFIG_USART6_RXDMA) && defined(CONFIG_USART6_RS485)) || \ + (defined(CONFIG_UART7_RXDMA) && defined(CONFIG_UART7_RS485)) || \ + (defined(CONFIG_UART8_RXDMA) && defined(CONFIG_UART8_RS485)) # error "RXDMA and RS-485 cannot be enabled at the same time for the same U[S]ART" # endif @@ -138,6 +141,14 @@ # error "USART6 DMA channel not defined (DMAMAP_USART6_RX)" # endif +# if defined(CONFIG_UART7_RXDMA) && !defined(DMAMAP_UART7_RX) +# error "UART7 DMA channel not defined (DMAMAP_UART7_RX)" +# endif + +# if defined(CONFIG_UART8_RXDMA) && !defined(DMAMAP_UART8_RX) +# error "UART8 DMA channel not defined (DMAMAP_UART8_RX)" +# endif + # elif defined(CONFIG_STM32_STM32F10XX) # if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \ @@ -337,6 +348,12 @@ static int up_interrupt_uart5(int irq, void *context); #ifdef CONFIG_STM32_USART6 static int up_interrupt_usart6(int irq, void *context); #endif +#ifdef CONFIG_STM32_UART7 +static int up_interrupt_uart7(int irq, void *context); +#endif +#ifdef CONFIG_STM32_UART8 +static int up_interrupt_uart8(int irq, void *context); +#endif /**************************************************************************** * Private Variables @@ -428,6 +445,22 @@ static char g_usart6rxfifo[RXDMA_BUFFER_SIZE]; # endif #endif +#ifdef CONFIG_STM32_UART7 +static char g_uart7rxbuffer[CONFIG_UART7_RXBUFSIZE]; +static char g_uart7txbuffer[CONFIG_UART7_TXBUFSIZE]; +# ifdef CONFIG_UART7_RXDMA +static char g_uart7rxfifo[RXDMA_BUFFER_SIZE]; +# endif +#endif + +#ifdef CONFIG_STM32_UART8 +static char g_uart8rxbuffer[CONFIG_UART8_RXBUFSIZE]; +static char g_uart8txbuffer[CONFIG_UART8_TXBUFSIZE]; +# ifdef CONFIG_UART8_RXDMA +static char g_uart8rxfifo[RXDMA_BUFFER_SIZE]; +# endif +#endif + /* This describes the state of the STM32 USART1 ports. */ #ifdef CONFIG_STM32_USART1 @@ -792,6 +825,126 @@ static struct up_dev_s g_usart6priv = }; #endif +/* This describes the state of the STM32 UART7 port. */ + +#ifdef CONFIG_STM32_UART7 +static struct up_dev_s g_uart7priv = +{ + .dev = + { +#if CONSOLE_UART == 7 + .isconsole = true, +#endif + .recv = + { + .size = CONFIG_UART7_RXBUFSIZE, + .buffer = g_uart7rxbuffer, + }, + .xmit = + { + .size = CONFIG_UART7_TXBUFSIZE, + .buffer = g_uart7txbuffer, + }, +#ifdef CONFIG_UART7_RXDMA + .ops = &g_uart_dma_ops, +#else + .ops = &g_uart_ops, +#endif + .priv = &g_uart7priv, + }, + + .irq = STM32_IRQ_UART7, + .parity = CONFIG_UART7_PARITY, + .bits = CONFIG_UART7_BITS, + .stopbits2 = CONFIG_UART7_2STOP, + .baud = CONFIG_UART7_BAUD, + .apbclock = STM32_PCLK1_FREQUENCY, + .usartbase = STM32_UART7_BASE, + .tx_gpio = GPIO_UART7_TX, + .rx_gpio = GPIO_UART7_RX, +#ifdef GPIO_UART7_CTS + .cts_gpio = GPIO_UART7_CTS, +#endif +#ifdef GPIO_UART7_RTS + .rts_gpio = GPIO_UART7_RTS, +#endif +#ifdef CONFIG_UART7_RXDMA + .rxdma_channel = DMAMAP_UART7_RX, + .rxfifo = g_uart7rxfifo, +#endif + .vector = up_interrupt_uart7, + +#ifdef CONFIG_UART7_RS485 + .rs485_dir_gpio = GPIO_UART7_RS485_DIR, +# if (CONFIG_UART7_RS485_DIR_POLARITY == 0) + .rs485_dir_polarity = false, +# else + .rs485_dir_polarity = true, +# endif +#endif +}; +#endif + +/* This describes the state of the STM32 UART8 port. */ + +#ifdef CONFIG_STM32_UART8 +static struct up_dev_s g_uart8priv = +{ + .dev = + { +#if CONSOLE_UART == 8 + .isconsole = true, +#endif + .recv = + { + .size = CONFIG_UART8_RXBUFSIZE, + .buffer = g_uart8rxbuffer, + }, + .xmit = + { + .size = CONFIG_UART8_TXBUFSIZE, + .buffer = g_uart8txbuffer, + }, +#ifdef CONFIG_UART8_RXDMA + .ops = &g_uart_dma_ops, +#else + .ops = &g_uart_ops, +#endif + .priv = &g_uart8priv, + }, + + .irq = STM32_IRQ_UART8, + .parity = CONFIG_UART8_PARITY, + .bits = CONFIG_UART8_BITS, + .stopbits2 = CONFIG_UART8_2STOP, + .baud = CONFIG_UART8_BAUD, + .apbclock = STM32_PCLK1_FREQUENCY, + .usartbase = STM32_UART8_BASE, + .tx_gpio = GPIO_UART8_TX, + .rx_gpio = GPIO_UART8_RX, +#ifdef GPIO_UART8_CTS + .cts_gpio = GPIO_UART8_CTS, +#endif +#ifdef GPIO_UART8_RTS + .rts_gpio = GPIO_UART8_RTS, +#endif +#ifdef CONFIG_UART8_RXDMA + .rxdma_channel = DMAMAP_UART8_RX, + .rxfifo = g_uart8rxfifo, +#endif + .vector = up_interrupt_uart8, + +#ifdef CONFIG_UART8_RS485 + .rs485_dir_gpio = GPIO_UART8_RS485_DIR, +# if (CONFIG_UART8_RS485_DIR_POLARITY == 0) + .rs485_dir_polarity = false, +# else + .rs485_dir_polarity = true, +# endif +#endif +}; +#endif + /* This table lets us iterate over the configured USARTs */ static struct up_dev_s *uart_devs[STM32_NUSART] = @@ -814,6 +967,12 @@ static struct up_dev_s *uart_devs[STM32_NUSART] = #ifdef CONFIG_STM32_USART6 [5] = &g_usart6priv, #endif +#ifdef CONFIG_STM32_UART7 + [6] = &g_uart7priv, +#endif +#ifdef CONFIG_STM32_UART8 + [7] = &g_uart8priv, +#endif }; #ifdef CONFIG_PM @@ -1899,6 +2058,20 @@ static int up_interrupt_usart6(int irq, void *context) } #endif +#ifdef CONFIG_STM32_UART7 +static int up_interrupt_uart7(int irq, void *context) +{ + return up_interrupt_common(&g_uart7priv); +} +#endif + +#ifdef CONFIG_STM32_UART8 +static int up_interrupt_uart8(int irq, void *context) +{ + return up_interrupt_common(&g_uart8priv); +} +#endif + /**************************************************************************** * Name: up_dma_rxcallback * @@ -2187,6 +2360,20 @@ void stm32_serial_dma_poll(void) } #endif +#ifdef CONFIG_UART7_RXDMA + if (g_uart7priv.rxdma != NULL) + { + up_dma_rxcallback(g_uart7priv.rxdma, 0, &g_uart7priv); + } +#endif + +#ifdef CONFIG_UART8_RXDMA + if (g_uart8priv.rxdma != NULL) + { + up_dma_rxcallback(g_uart8priv.rxdma, 0, &g_uart8priv); + } +#endif + irqrestore(flags); } #endif diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.c b/nuttx/arch/arm/src/stm32/stm32_spi.c index b4a4f36ab..c8a8faa66 100644 --- a/nuttx/arch/arm/src/stm32/stm32_spi.c +++ b/nuttx/arch/arm/src/stm32/stm32_spi.c @@ -82,7 +82,8 @@ #include "stm32_dma.h" #include "stm32_spi.h" -#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3) +#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3) || \ + defined(CONFIG_STM32_SPI4) || defined(CONFIG_STM32_SPI5) || defined(CONFIG_STM32_SPI6) /************************************************************************************ * Definitions @@ -377,6 +378,123 @@ static struct stm32_spidev_s g_spi3dev = }; #endif +#ifdef CONFIG_STM32_SPI4 +static const struct spi_ops_s g_sp4iops = +{ +#ifndef CONFIG_SPI_OWNBUS + .lock = spi_lock, +#endif + .select = stm32_spi4select, + .setfrequency = spi_setfrequency, + .setmode = spi_setmode, + .setbits = spi_setbits, + .status = stm32_spi4status, +#ifdef CONFIG_SPI_CMDDATA + .cmddata = stm32_spi4cmddata, +#endif + .send = spi_send, +#ifdef CONFIG_SPI_EXCHANGE + .exchange = spi_exchange, +#else + .sndblock = spi_sndblock, + .recvblock = spi_recvblock, +#endif + .registercallback = 0, +}; + +static struct stm32_spidev_s g_spi4dev = +{ + .spidev = { &g_sp4iops }, + .spibase = STM32_SPI4_BASE, + .spiclock = STM32_PCLK1_FREQUENCY, +#ifdef CONFIG_STM32_SPI_INTERRUPTS + .spiirq = STM32_IRQ_SPI4, +#endif +#ifdef CONFIG_STM32_SPI_DMA + .rxch = DMACHAN_SPI4_RX, + .txch = DMACHAN_SPI4_TX, +#endif +}; +#endif + +#ifdef CONFIG_STM32_SPI5 +static const struct spi_ops_s g_sp5iops = +{ +#ifndef CONFIG_SPI_OWNBUS + .lock = spi_lock, +#endif + .select = stm32_spi5select, + .setfrequency = spi_setfrequency, + .setmode = spi_setmode, + .setbits = spi_setbits, + .status = stm32_spi5status, +#ifdef CONFIG_SPI_CMDDATA + .cmddata = stm32_spi5cmddata, +#endif + .send = spi_send, +#ifdef CONFIG_SPI_EXCHANGE + .exchange = spi_exchange, +#else + .sndblock = spi_sndblock, + .recvblock = spi_recvblock, +#endif + .registercallback = 0, +}; + +static struct stm32_spidev_s g_spi5dev = +{ + .spidev = { &g_sp5iops }, + .spibase = STM32_SPI5_BASE, + .spiclock = STM32_PCLK1_FREQUENCY, +#ifdef CONFIG_STM32_SPI_INTERRUPTS + .spiirq = STM32_IRQ_SPI5, +#endif +#ifdef CONFIG_STM32_SPI_DMA + .rxch = DMACHAN_SPI5_RX, + .txch = DMACHAN_SPI5_TX, +#endif +}; +#endif + +#ifdef CONFIG_STM32_SPI6 +static const struct spi_ops_s g_sp6iops = +{ +#ifndef CONFIG_SPI_OWNBUS + .lock = spi_lock, +#endif + .select = stm32_spi6select, + .setfrequency = spi_setfrequency, + .setmode = spi_setmode, + .setbits = spi_setbits, + .status = stm32_spi6status, +#ifdef CONFIG_SPI_CMDDATA + .cmddata = stm32_spi3cmddata, +#endif + .send = spi_send, +#ifdef CONFIG_SPI_EXCHANGE + .exchange = spi_exchange, +#else + .sndblock = spi_sndblock, + .recvblock = spi_recvblock, +#endif + .registercallback = 0, +}; + +static struct stm32_spidev_s g_spi6dev = +{ + .spidev = { &g_sp6iops }, + .spibase = STM32_SPI6_BASE, + .spiclock = STM32_PCLK1_FREQUENCY, +#ifdef CONFIG_STM32_SPI_INTERRUPTS + .spiirq = STM32_IRQ_SPI6, +#endif +#ifdef CONFIG_STM32_SPI_DMA + .rxch = DMACHAN_SPI6_RX, + .txch = DMACHAN_SPI6_TX, +#endif +}; +#endif + /************************************************************************************ * Public Data ************************************************************************************/ @@ -1464,6 +1582,78 @@ FAR struct spi_dev_s *up_spiinitialize(int port) } else #endif +#ifdef CONFIG_STM32_SPI4 + if (port == 4) + { + /* Select SPI4 */ + + priv = &g_spi4dev; + + /* Only configure if the port is not already configured */ + + if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure SPI4 pins: SCK, MISO, and MOSI */ + + stm32_configgpio(GPIO_SPI4_SCK); + stm32_configgpio(GPIO_SPI4_MISO); + stm32_configgpio(GPIO_SPI4_MOSI); + + /* Set up default configuration: Master, 8-bit, etc. */ + + spi_portinitialize(priv); + } + } + else +#endif +#ifdef CONFIG_STM32_SPI5 + if (port == 5) + { + /* Select SPI5 */ + + priv = &g_spi5dev; + + /* Only configure if the port is not already configured */ + + if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure SPI5 pins: SCK, MISO, and MOSI */ + + stm32_configgpio(GPIO_SPI5_SCK); + stm32_configgpio(GPIO_SPI5_MISO); + stm32_configgpio(GPIO_SPI5_MOSI); + + /* Set up default configuration: Master, 8-bit, etc. */ + + spi_portinitialize(priv); + } + } + else +#endif +#ifdef CONFIG_STM32_SPI6 + if (port == 6) + { + /* Select SPI6 */ + + priv = &g_spi6dev; + + /* Only configure if the port is not already configured */ + + if ((spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE) == 0) + { + /* Configure SPI6 pins: SCK, MISO, and MOSI */ + + stm32_configgpio(GPIO_SPI6_SCK); + stm32_configgpio(GPIO_SPI6_MISO); + stm32_configgpio(GPIO_SPI6_MOSI); + + /* Set up default configuration: Master, 8-bit, etc. */ + + spi_portinitialize(priv); + } + } + else +#endif { spidbg("ERROR: Unsupported SPI port: %d\n", port); return NULL; @@ -1473,4 +1663,4 @@ FAR struct spi_dev_s *up_spiinitialize(int port) return (FAR struct spi_dev_s *)priv; } -#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 || CONFIG_STM32_SPI3 */ +#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 || CONFIG_STM32_SPI3 || CONFIG_STM32_SPI4 || CONFIG_STM32_SPI5 || CONFIG_STM32_SPI6 */ diff --git a/nuttx/arch/arm/src/stm32/stm32_spi.h b/nuttx/arch/arm/src/stm32/stm32_spi.h index 6030ddfdd..58c3f0da7 100644 --- a/nuttx/arch/arm/src/stm32/stm32_spi.h +++ b/nuttx/arch/arm/src/stm32/stm32_spi.h @@ -71,11 +71,11 @@ enum spi_dev_e; ************************************************************************************/ /************************************************************************************ - * Name: stm32_spi1/2/3select and stm32_spi1/2/3status + * Name: stm32_spi1/2/...select and stm32_spi1/2/...status * * Description: - * The external functions, stm32_spi1/2/3select, stm32_spi1/2/3status, and - * stm32_spi1/2/3cmddata must be provided by board-specific logic. These are + * The external functions, stm32_spi1/2/...select, stm32_spi1/2/...status, and + * stm32_spi1/2/...cmddata must be provided by board-specific logic. These are * implementations of the select, status, and cmddata methods of the SPI interface * defined by struct spi_ops_s (see include/nuttx/spi.h). All other methods * (including up_spiinitialize()) are provided by common STM32 logic. To use this @@ -83,11 +83,11 @@ enum spi_dev_e; * * 1. Provide logic in stm32_boardinitialize() to configure SPI chip select * pins. - * 2. Provide stm32_spi1/2/3select() and stm32_spi1/2/3status() functions in your + * 2. Provide stm32_spi1/2/...select() and stm32_spi1/2/...status() functions in your * board-specific logic. These functions will perform chip selection and * status operations using GPIOs in the way your board is configured. * 3. If CONFIG_SPI_CMDDATA is defined in your NuttX configuration file, then - * provide stm32_spi1/2/3cmddata() functions in your board-specific logic. + * provide stm32_spi1/2/...cmddata() functions in your board-specific logic. * These functions will perform cmd/data selection operations using GPIOs in the * way your board is configured. * 4. Add a calls to up_spiinitialize() in your low level application @@ -111,6 +111,18 @@ EXTERN void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, b EXTERN uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); EXTERN int stm32_spi3cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); +EXTERN void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected); +EXTERN uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); +EXTERN int stm32_spi4cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); + +EXTERN void stm32_spi5select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected); +EXTERN uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); +EXTERN int stm32_spi5cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); + +EXTERN void stm32_spi6select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected); +EXTERN uint8_t stm32_spi6status(FAR struct spi_dev_s *dev, enum spi_dev_e devid); +EXTERN int stm32_spi6cmddata(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool cmd); + #undef EXTERN #if defined(__cplusplus) } diff --git a/nuttx/arch/arm/src/stm32/stm32_uart.h b/nuttx/arch/arm/src/stm32/stm32_uart.h index a26ea2009..e77122d82 100644 --- a/nuttx/arch/arm/src/stm32/stm32_uart.h +++ b/nuttx/arch/arm/src/stm32/stm32_uart.h @@ -52,6 +52,12 @@ * the device. */ +#if STM32_NUSART < 8 +# undef CONFIG_STM32_UART8 +#endif +#if STM32_NUSART < 7 +# undef CONFIG_STM32_UART7 +#endif #if STM32_NUSART < 6 # undef CONFIG_STM32_USART6 #endif @@ -75,7 +81,8 @@ #if defined(CONFIG_STM32_USART1) || defined(CONFIG_STM32_USART2) || \ defined(CONFIG_STM32_USART3) || defined(CONFIG_STM32_UART4) || \ - defined(CONFIG_STM32_UART5) || defined(CONFIG_STM32_USART6) + defined(CONFIG_STM32_UART5) || defined(CONFIG_STM32_USART6) || \ + defined(CONFIG_STM32_UART7) || defined(CONFIG_STM32_UART8) # define HAVE_UART 1 #endif @@ -87,6 +94,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 1 # define HAVE_CONSOLE 1 #elif defined(CONFIG_USART2_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART2) @@ -95,6 +104,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 2 # define HAVE_CONSOLE 1 #elif defined(CONFIG_USART3_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART3) @@ -103,6 +114,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 3 # define HAVE_CONSOLE 1 #elif defined(CONFIG_UART4_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART4) @@ -111,6 +124,8 @@ # undef CONFIG_USART3_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 4 # define HAVE_CONSOLE 1 #elif defined(CONFIG_UART5_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART5) @@ -119,6 +134,8 @@ # undef CONFIG_USART3_SERIAL_CONSOLE # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 5 # define HAVE_CONSOLE 1 #elif defined(CONFIG_USART6_SERIAL_CONSOLE) && defined(CONFIG_STM32_USART6) @@ -127,8 +144,31 @@ # undef CONFIG_USART3_SERIAL_CONSOLE # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 6 # define HAVE_CONSOLE 1 +#elif defined(CONFIG_UART7_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART7) +# undef CONFIG_USART1_SERIAL_CONSOLE +# undef CONFIG_USART2_SERIAL_CONSOLE +# undef CONFIG_USART3_SERIAL_CONSOLE +# undef CONFIG_UART4_SERIAL_CONSOLE +# undef CONFIG_UART5_SERIAL_CONSOLE +# undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART5_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE +# define CONSOLE_UART 7 +# define HAVE_CONSOLE 1 +#elif defined(CONFIG_UART8_SERIAL_CONSOLE) && defined(CONFIG_STM32_UART8) +# undef CONFIG_USART1_SERIAL_CONSOLE +# undef CONFIG_USART2_SERIAL_CONSOLE +# undef CONFIG_USART3_SERIAL_CONSOLE +# undef CONFIG_UART4_SERIAL_CONSOLE +# undef CONFIG_UART6_SERIAL_CONSOLE +# undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# define CONSOLE_UART 8 +# define HAVE_CONSOLE 1 #else # undef CONFIG_USART1_SERIAL_CONSOLE # undef CONFIG_USART2_SERIAL_CONSOLE @@ -136,6 +176,8 @@ # undef CONFIG_UART4_SERIAL_CONSOLE # undef CONFIG_UART5_SERIAL_CONSOLE # undef CONFIG_USART6_SERIAL_CONSOLE +# undef CONFIG_UART7_SERIAL_CONSOLE +# undef CONFIG_UART8_SERIAL_CONSOLE # define CONSOLE_UART 0 # undef HAVE_CONSOLE #endif @@ -149,6 +191,8 @@ # undef CONFIG_UART4_RXDMA # undef CONFIG_UART5_RXDMA # undef CONFIG_USART6_RXDMA +# undef CONFIG_UART7_RXDMA +# undef CONFIG_UART8_RXDMA #endif /* Disable the DMA configuration on all unused USARTs */ @@ -177,12 +221,21 @@ # undef CONFIG_USART6_RXDMA #endif +#ifndef CONFIG_STM32_UART7 +# undef CONFIG_UART7_RXDMA +#endif + +#ifndef CONFIG_STM32_UART8 +# undef CONFIG_UART8_RXDMA +#endif + /* Is DMA available on any (enabled) USART? */ #undef SERIAL_HAVE_DMA #if defined(CONFIG_USART1_RXDMA) || defined(CONFIG_USART2_RXDMA) || \ - defined(CONFIG_USART3_RXDMA) || defined(CONFIG_UART4_RXDMA) || \ - defined(CONFIG_UART5_RXDMA) || defined(CONFIG_USART6_RXDMA) + defined(CONFIG_USART3_RXDMA) || defined(CONFIG_UART4_RXDMA) || \ + defined(CONFIG_UART5_RXDMA) || defined(CONFIG_USART6_RXDMA) || \ + defined(CONFIG_UART7_RXDMA) || defined(CONFIG_UART8_RXDMA) # define SERIAL_HAVE_DMA 1 #endif @@ -201,6 +254,10 @@ # define SERIAL_HAVE_CONSOLE_DMA 1 #elif defined(CONFIG_USART6_SERIAL_CONSOLE) && defined(CONFIG_USART6_RXDMA) # define SERIAL_HAVE_CONSOLE_DMA 1 +#elif defined(CONFIG_UART7_SERIAL_CONSOLE) && defined(CONFIG_UART7_RXDMA) +# define SERIAL_HAVE_CONSOLE_DMA 1 +#elif defined(CONFIG_UART8_SERIAL_CONSOLE) && defined(CONFIG_UART8_RXDMA) +# define SERIAL_HAVE_CONSOLE_DMA 1 #endif /* Is DMA used on all (enabled) USARTs */ @@ -218,13 +275,18 @@ # undef SERIAL_HAVE_ONLY_DMA #elif defined(CONFIG_STM32_USART6) && !defined(CONFIG_USART6_RXDMA) # undef SERIAL_HAVE_ONLY_DMA +#elif defined(CONFIG_STM32_UART7) && !defined(CONFIG_UART7_RXDMA) +# undef SERIAL_HAVE_ONLY_DMA +#elif defined(CONFIG_STM32_UART8) && !defined(CONFIG_UART8_RXDMA) +# undef SERIAL_HAVE_ONLY_DMA #endif /* Is RS-485 used? */ #if defined(CONFIG_USART1_RS485) || defined(CONFIG_USART2_RS485) || \ defined(CONFIG_USART3_RS485) || defined(CONFIG_UART4_RS485) || \ - defined(CONFIG_UART5_RS485) || defined(CONFIG_USART6_RS485) + defined(CONFIG_UART5_RS485) || defined(CONFIG_USART6_RS485) || \ + defined(CONFIG_UART7_RS485) || defined(CONFIG_UART8_RS485) # define HAVE_RS485 1 #endif diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c index c6c0b2382..fc7fe1697 100644 --- a/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c +++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_rcc.c @@ -437,6 +437,19 @@ static inline void rcc_enableapb1(void) regval |= RCC_APB1ENR_DACEN; #endif +#ifdef CONFIG_STM32_UART7 + /* UART7 clock enable */ + + regval |= RCC_APB1ENR_UART7EN; +#endif + +#ifdef CONFIG_STM32_UART8 + /* UART8 clock enable */ + + regval |= RCC_APB1ENR_UART8EN; +#endif + + putreg32(regval, STM32_RCC_APB1ENR); /* Enable peripherals */ } @@ -512,6 +525,12 @@ static inline void rcc_enableapb2(void) regval |= RCC_APB2ENR_SPI1EN; #endif +#ifdef CONFIG_STM32_SPI4 + /* SPI4 clock enable */ + + regval |= RCC_APB2ENR_SPI4EN; +#endif + #ifdef CONFIG_STM32_SYSCFG /* System configuration controller clock enable */ @@ -536,6 +555,18 @@ static inline void rcc_enableapb2(void) regval |= RCC_APB2ENR_TIM11EN; #endif +#ifdef CONFIG_STM32_SPI5 + /* SPI5 clock enable */ + + regval |= RCC_APB2ENR_SPI5EN; +#endif + +#ifdef CONFIG_STM32_SPI6 + /* SPI6 clock enable */ + + regval |= RCC_APB2ENR_SPI6EN; +#endif + putreg32(regval, STM32_RCC_APB2ENR); /* Enable peripherals */ } @@ -591,7 +622,12 @@ static void stm32_stdclockconfig(void) putreg32(regval, STM32_RCC_APB1ENR); regval = getreg32(STM32_PWR_CR); +#if defined(CONFIG_STM32_STM32F427) + regval &= ~PWR_CR_VOS_MASK; + regval |= PWR_CR_VOS_SCALE_1; +#else regval |= PWR_CR_VOS; +#endif putreg32(regval, STM32_PWR_CR); /* Set the HCLK source/divider */ diff --git a/nuttx/drivers/serial/Kconfig b/nuttx/drivers/serial/Kconfig index 119923a69..e91246612 100644 --- a/nuttx/drivers/serial/Kconfig +++ b/nuttx/drivers/serial/Kconfig @@ -292,6 +292,10 @@ config ARCH_HAVE_UART5 bool config ARCH_HAVE_UART6 bool +config ARCH_HAVE_UART7 + bool +config ARCH_HAVE_UART8 + bool config ARCH_HAVE_USART0 bool @@ -307,6 +311,10 @@ config ARCH_HAVE_USART5 bool config ARCH_HAVE_USART6 bool +config ARCH_HAVE_USART7 + bool +config ARCH_HAVE_USART8 + bool config MCU_SERIAL bool @@ -403,6 +411,22 @@ config USART6_SERIAL_CONSOLE bool "USART6" depends on ARCH_HAVE_USART6 +config UART7_SERIAL_CONSOLE + bool "UART7" + depends on ARCH_HAVE_UART7 + +config USART7_SERIAL_CONSOLE + bool "USART7" + depends on ARCH_HAVE_USART7 + +config UART8_SERIAL_CONSOLE + bool "UART8" + depends on ARCH_HAVE_UART8 + +config USART8_SERIAL_CONSOLE + bool "USART8" + depends on ARCH_HAVE_USART8 + config NO_SERIAL_CONSOLE bool "No serial console" @@ -1052,3 +1076,173 @@ config UART6_2STOP 1=Two stop bits endmenu + +menu "USART7 Configuration" + depends on ARCH_HAVE_USART7 + +config USART7_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config USART7_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config USART7_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the USART. + +config USART7_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config USART7_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config USART7_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +endmenu + +menu "UART7 Configuration" + depends on ARCH_HAVE_UART7 + +config UART7_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config UART7_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config UART7_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the UART. + +config UART7_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config UART7_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config UART7_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +menu "USART8 Configuration" + depends on ARCH_HAVE_USART8 + +config USART8_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config USART8_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config USART8_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the USART. + +config USART8_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config USART8_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config USART8_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +endmenu + +menu "UART8 Configuration" + depends on ARCH_HAVE_UART8 + +config UART8_RXBUFSIZE + int "Receive buffer size" + default 256 + help + Characters are buffered as they are received. This specifies + the size of the receive buffer. + +config UART8_TXBUFSIZE + int "Transmit buffer size" + default 256 + help + Characters are buffered before being sent. This specifies + the size of the transmit buffer. + +config UART8_BAUD + int "BAUD rate" + default 115200 + help + The configured BAUD of the UART. + +config UART8_BITS + int "Character size" + default 8 + help + The number of bits. Must be either 7 or 8. + +config UART8_PARITY + int "Parity setting" + default 0 + help + 0=no parity, 1=odd parity, 2=even parity + +config UART8_2STOP + int "Uses 2 stop bits" + default 0 + help + 1=Two stop bits + +endmenu -- cgit v1.2.3 From f243f6ef66cb17882a0ee8645a8a7c639272c753 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 1 Apr 2013 01:23:05 -0700 Subject: Scratch in a mostly-building board config for fmuv2 --- apps/drivers/boards/px4fmuv2/Makefile | 41 + apps/drivers/boards/px4fmuv2/px4fmu_can.c | 144 ++++ apps/drivers/boards/px4fmuv2/px4fmu_init.c | 224 +++++ apps/drivers/boards/px4fmuv2/px4fmu_internal.h | 106 +++ apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 105 +++ apps/drivers/boards/px4fmuv2/px4fmu_spi.c | 141 +++ apps/drivers/boards/px4fmuv2/px4fmu_usb.c | 108 +++ apps/drivers/drv_gpio.h | 5 + apps/drivers/stm32/drv_hrt.c | 2 +- apps/sensors/sensors.cpp | 2 + makefiles/config_px4fmuv2_default.mk | 62 ++ nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h | 10 +- nuttx/configs/px4fmuv2/Kconfig | 7 + nuttx/configs/px4fmuv2/common/Make.defs | 184 ++++ nuttx/configs/px4fmuv2/common/ld.script | 150 ++++ nuttx/configs/px4fmuv2/include/board.h | 351 ++++++++ nuttx/configs/px4fmuv2/include/nsh_romfsimg.h | 42 + nuttx/configs/px4fmuv2/nsh/Make.defs | 3 + nuttx/configs/px4fmuv2/nsh/appconfig | 143 +++ nuttx/configs/px4fmuv2/nsh/defconfig | 1050 +++++++++++++++++++++++ nuttx/configs/px4fmuv2/nsh/setenv.sh | 67 ++ nuttx/configs/px4fmuv2/src/Makefile | 84 ++ nuttx/configs/px4fmuv2/src/empty.c | 4 + 23 files changed, 3029 insertions(+), 6 deletions(-) create mode 100644 apps/drivers/boards/px4fmuv2/Makefile create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_can.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_init.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_internal.h create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_spi.c create mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_usb.c create mode 100644 makefiles/config_px4fmuv2_default.mk create mode 100644 nuttx/configs/px4fmuv2/Kconfig create mode 100644 nuttx/configs/px4fmuv2/common/Make.defs create mode 100644 nuttx/configs/px4fmuv2/common/ld.script create mode 100755 nuttx/configs/px4fmuv2/include/board.h create mode 100644 nuttx/configs/px4fmuv2/include/nsh_romfsimg.h create mode 100644 nuttx/configs/px4fmuv2/nsh/Make.defs create mode 100644 nuttx/configs/px4fmuv2/nsh/appconfig create mode 100755 nuttx/configs/px4fmuv2/nsh/defconfig create mode 100755 nuttx/configs/px4fmuv2/nsh/setenv.sh create mode 100644 nuttx/configs/px4fmuv2/src/Makefile create mode 100644 nuttx/configs/px4fmuv2/src/empty.c diff --git a/apps/drivers/boards/px4fmuv2/Makefile b/apps/drivers/boards/px4fmuv2/Makefile new file mode 100644 index 000000000..9c04a8c42 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/Makefile @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4FMUv2 +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4fmuv2 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_can.c b/apps/drivers/boards/px4fmuv2/px4fmu_can.c new file mode 100644 index 000000000..86ba183b8 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "px4fmu_internal.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/apps/drivers/boards/px4fmuv2/px4fmu_init.c new file mode 100644 index 000000000..c9364c2a4 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_init.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + int result; + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + // initial LED state + // XXX need to make this work again +// drv_led_start(); + up_ledoff(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + /* Get the SPI port for the FRAM */ + + message("[boot] Initializing SPI port 2\n"); + spi2 = up_spiinitialize(3); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 2\n"); + + /* XXX need a driver to bind the FRAM to */ + + //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + stm32_configgpio(GPIO_ADC1_IN12); + + return OK; +} diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h new file mode 100644 index 000000000..001b23cb2 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 px4fmu_internal.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) + +/* External interrupts */ + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c new file mode 100644 index 000000000..14e4052be --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_spi.c b/apps/drivers/boards/px4fmuv2/px4fmu_spi.c new file mode 100644 index 000000000..f90f25071 --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_spi.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_usb.c b/apps/drivers/boards/px4fmuv2/px4fmu_usb.c new file mode 100644 index 000000000..b0b669fbe --- /dev/null +++ b/apps/drivers/boards/px4fmuv2/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index 92d184326..2fa6d8b8e 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -42,6 +42,11 @@ #include +#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#warning No GPIOs on this board. +#define GPIO_DEVICE_PATH "/dev/null" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4FMU /* * PX4FMU GPIO numbers. diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index bb67d5e6d..cec0c49ff 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -125,7 +125,7 @@ # define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC # define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN # if CONFIG_STM32_TIM8 -# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8 # endif #elif HRT_TIMER == 9 # define HRT_TIMER_BASE STM32_TIM9_BASE diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 1f3f7707e..123bbb120 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -372,7 +372,9 @@ Sensors *g_sensors; } Sensors::Sensors() : +#ifdef CONFIG_HRT_PPM _ppm_last_valid(0), +#endif _fd_adc(-1), _last_adc(0), diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk new file mode 100644 index 000000000..d5bc758b8 --- /dev/null +++ b/makefiles/config_px4fmuv2_default.mk @@ -0,0 +1,62 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, adc, , 2048, adc_main ) \ + $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ + $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ + $(call _B, bl_update, , 4096, bl_update_main ) \ + $(call _B, blinkm, , 2048, blinkm_main ) \ + $(call _B, boardinfo, , 2048, boardinfo_main ) \ + $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ + $(call _B, control_demo, , 2048, control_demo_main ) \ + $(call _B, delay_test, , 2048, delay_test_main ) \ + $(call _B, eeprom, , 4096, eeprom_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, fmu, , 2048, fmu_main ) \ + $(call _B, gps, , 2048, gps_main ) \ + $(call _B, hil, , 2048, hil_main ) \ + $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ + $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ + $(call _B, l3gd20, , 2048, l3gd20_main ) \ + $(call _B, math_demo, , 8192, math_demo_main ) \ + $(call _B, mavlink, , 2048, mavlink_main ) \ + $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ + $(call _B, mixer, , 4096, mixer_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, tests, , 12000, tests_main ) \ + $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ + $(call _B, top, SCHED_PRIORITY_DEFAULT-10, 3000, top_main ) \ + $(call _B, uorb, , 4096, uorb_main ) diff --git a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h index dca3820fd..55133b3da 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32_syscfg.h @@ -89,11 +89,11 @@ /* SYSCFG peripheral mode configuration register */ #define SYSCFG_PMC_MII_RMII_SEL (1 << 23) /* Bit 23: Ethernet PHY interface selection */ -+#ifdef CONFIG_STM32_STM32F427 -+# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */ -+# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */ -+# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */ -+#endif +#ifdef CONFIG_STM32_STM32F427 +# define SYSCFG_PMC_ADC3DC2 (1 << 18) /* Bit 18: See AN4073 */ +# define SYSCFG_PMC_ADC2DC2 (1 << 17) /* Bit 17: See AN4073 */ +# define SYSCFG_PMC_ADC1DC2 (1 << 16) /* Bit 16: See AN4073 */ +#endif /* SYSCFG external interrupt configuration register 1-4 */ diff --git a/nuttx/configs/px4fmuv2/Kconfig b/nuttx/configs/px4fmuv2/Kconfig new file mode 100644 index 000000000..a10a02ba4 --- /dev/null +++ b/nuttx/configs/px4fmuv2/Kconfig @@ -0,0 +1,7 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMUV2 +endif diff --git a/nuttx/configs/px4fmuv2/common/Make.defs b/nuttx/configs/px4fmuv2/common/Make.defs new file mode 100644 index 000000000..be387dce1 --- /dev/null +++ b/nuttx/configs/px4fmuv2/common/Make.defs @@ -0,0 +1,184 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMUV2 +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx/configs/px4fmuv2/common/ld.script b/nuttx/configs/px4fmuv2/common/ld.script new file mode 100644 index 000000000..1be39fb87 --- /dev/null +++ b/nuttx/configs/px4fmuv2/common/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h new file mode 100755 index 000000000..286dedab0 --- /dev/null +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -0,0 +1,351 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* High-resolution timer + */ +#ifdef CONFIG_HRT_TIMER +# define HRT_TIMER 8 /* use timer8 for the HRT */ +# define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#endif + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ +#define GPIO_USART1_TX 0 /* USART1 is RX-only */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + * + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * I2C busses + */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x5a + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 2 /* timer 3 */ +#define TONE_ALARM_CHANNEL 1 /* channel 3 */ +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h b/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx/configs/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs new file mode 100644 index 000000000..3e6f88bd3 --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu/common/Make.defs diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig new file mode 100644 index 000000000..fbcb67dcf --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -0,0 +1,143 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +# System library - utility functions +CONFIGURED_APPS += systemlib +CONFIGURED_APPS += systemlib/mixer + +# Math library +ifneq ($(CONFIG_APM),y) +CONFIGURED_APPS += mathlib +CONFIGURED_APPS += mathlib/CMSIS +CONFIGURED_APPS += examples/math_demo +endif + +# Control library +ifneq ($(CONFIG_APM),y) +CONFIGURED_APPS += controllib +CONFIGURED_APPS += examples/control_demo +CONFIGURED_APPS += examples/kalman_demo +endif + +# System utility commands +CONFIGURED_APPS += systemcmds/reboot +CONFIGURED_APPS += systemcmds/perf +CONFIGURED_APPS += systemcmds/top +CONFIGURED_APPS += systemcmds/boardinfo +CONFIGURED_APPS += systemcmds/mixer +# No I2C EEPROM - need new param interface +#CONFIGURED_APPS += systemcmds/eeprom +#CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/pwm +CONFIGURED_APPS += systemcmds/bl_update +CONFIGURED_APPS += systemcmds/preflight_check +CONFIGURED_APPS += systemcmds/delay_test + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +# CONFIGURED_APPS += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/deamon +# CONFIGURED_APPS += examples/px4_deamon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +# CONFIGURED_APPS += examples/px4_mavlink_debug + +# Shared object broker; required by many parts of the system. +CONFIGURED_APPS += uORB + +CONFIGURED_APPS += mavlink +CONFIGURED_APPS += mavlink_onboard +CONFIGURED_APPS += commander +CONFIGURED_APPS += sdlog +CONFIGURED_APPS += sensors + +ifneq ($(CONFIG_APM),y) +#CONFIGURED_APPS += ardrone_interface +CONFIGURED_APPS += multirotor_att_control +CONFIGURED_APPS += multirotor_pos_control +#CONFIGURED_APPS += fixedwing_control +CONFIGURED_APPS += fixedwing_att_control +CONFIGURED_APPS += fixedwing_pos_control +CONFIGURED_APPS += position_estimator +CONFIGURED_APPS += attitude_estimator_ekf +CONFIGURED_APPS += hott_telemetry +endif + +# Hacking tools +# XXX needs updating for new i2c config +#CONFIGURED_APPS += systemcmds/i2c + +# Communication and Drivers +CONFIGURED_APPS += drivers/boards/px4fmuv2 +CONFIGURED_APPS += drivers/device +# XXX needs conversion to SPI +#CONFIGURED_APPS += drivers/ms5611 +CONFIGURED_APPS += drivers/l3gd20 +# XXX needs conversion to serial +#CONFIGURED_APPS += drivers/px4io +CONFIGURED_APPS += drivers/stm32 +#CONFIGURED_APPS += drivers/led +CONFIGURED_APPS += drivers/blinkm +CONFIGURED_APPS += drivers/stm32/tone_alarm +CONFIGURED_APPS += drivers/stm32/adc +#CONFIGURED_APPS += drivers/px4fmu +CONFIGURED_APPS += drivers/hil +CONFIGURED_APPS += drivers/gps +CONFIGURED_APPS += drivers/mb12xx + +# Testing stuff +CONFIGURED_APPS += px4/sensors_bringup +CONFIGURED_APPS += px4/tests + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig new file mode 100755 index 000000000..8e9feef65 --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -0,0 +1,1050 @@ +############################################################################ +# configs/px4fmu/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization +# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F427V=y +CONFIG_ARCH_BOARD="px4fmuv2" +CONFIG_ARCH_BOARD_PX4FMUV2=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DRAM_SIZE=0x00040000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_STM32_STM32F427=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# On-chip CCM SRAM configuration +# +# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need +# to do this if DMA is enabled to prevent non-DMA-able CCM memory from +# being a part of the stack. +# + +# +# On-board FSMC SRAM configuration +# +# CONFIG_STM32_FSMC - Required. See below +# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) +# +# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the +# FSMC (as opposed to an LCD or FLASH). +# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space +# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space +# +#CONFIG_STM32_FSMC_SRAM=n +#CONFIG_HEAP2_BASE=0x64000000 +#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) + +# +# Individual subsystems can be enabled: +# +# This set is exhaustive for PX4FMU and should be safe to cut and +# paste into any other config. +# +# AHB1: +CONFIG_STM32_CRC=n +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_ETHMAC=n +CONFIG_STM32_OTGHS=n +# AHB2: +CONFIG_STM32_DCMI=n +CONFIG_STM32_CRYP=n +CONFIG_STM32_HASH=n +CONFIG_STM32_RNG=n +CONFIG_STM32_OTGFS=y +# AHB3: +CONFIG_STM32_FSMC=n +# APB1: +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_TIM12=n +CONFIG_STM32_TIM13=n +CONFIG_STM32_TIM14=n +CONFIG_STM32_WWDG=y +CONFIG_STM32_IWDG=n +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI3=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART5=n +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=n +CONFIG_STM32_CAN1=n +CONFIG_STM32_CAN2=n +CONFIG_STM32_DAC=n +CONFIG_STM32_PWR=y +# APB2: +CONFIG_STM32_TIM1=y +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_USART6=y +# We use our own driver, but leave this on. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +CONFIG_STM32_ADC3=n +CONFIG_STM32_SDIO=n +CONFIG_STM32_SPI1=y +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y + +# +# Enable single wire support. If this is not defined, then this mode cannot +# be enabled. +# +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# We want the flash prefetch on for max performance. +# +STM32_FLASH_PREFETCH=y + +# +# STM32F40xxx specific serial device driver settings +# +# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, +# tcflush, etc.). If this is not defined, then the terminal settings (baud, +# parity, etc.) are not configurable at runtime; serial streams cannot be +# flushed, etc. +# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port +# immediately after creating the /dev/console device. This is required +# if the console serial port has RX DMA enabled. +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_CONSOLE_REINIT=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_UART8_SERIAL_CONSOLE=y + +#Mavlink messages can be bigger than 128 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=256 +CONFIG_USART3_TXBUFSIZE=256 +CONFIG_UART4_TXBUFSIZE=256 +CONFIG_USART6_TXBUFSIZE=128 +CONFIG_UART7_TXBUFSIZE=256 +CONFIG_UART8_TXBUFSIZE=256 + +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=256 +CONFIG_USART3_RXBUFSIZE=256 +CONFIG_UART4_RXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=256 +CONFIG_UART7_RXBUFSIZE=256 +CONFIG_UART8_RXBUFSIZE=256 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 +CONFIG_UART4_BAUD=115200 +CONFIG_USART6_BAUD=9600 +CONFIG_UART7_BAUD=115200 +CONFIG_UART8_BAUD=57600 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 +CONFIG_UART4_BITS=8 +CONFIG_USART6_BITS=8 +CONFIG_UART7_BITS=8 +CONFIG_UART8_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 +CONFIG_UART4_PARITY=0 +CONFIG_USART6_PARITY=0 +CONFIG_UART7_PARITY=0 +CONFIG_UART8_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 +CONFIG_UART4_2STOP=0 +CONFIG_USART6_2STOP=0 +CONFIG_UART7_2STOP=0 +CONFIG_UART8_2STOP=0 + +CONFIG_USART1_RXDMA=y +CONFIG_USART2_RXDMA=y +CONFIG_USART3_RXDMA=n +CONFIG_UART4_RXDMA=n +CONFIG_USART6_RXDMA=y +CONFIG_UART7_RXDMA=n +CONFIG_UART8_RXDMA=n + +# +# PX4FMU specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=n + +# +# STM32F40xxx specific SPI device driver settings +# +CONFIG_SPI_EXCHANGE=y +# DMA needs more work, not implemented on STM32F4x yet +#CONFIG_STM32_SPI_DMA=y + +# +# STM32F40xxx specific CAN device driver settings +# +# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or +# CONFIG_STM32_CAN2 must also be defined) +# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default +# Standard 11-bit IDs. +# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. +# Default: 8 +# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. +# Default: 4 +# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback +# mode for testing. The STM32 CAN driver does support loopback mode. +# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. +# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. +# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 +# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 +# +CONFIG_CAN=n +CONFIG_CAN_EXTID=n +#CONFIG_CAN_FIFOSIZE +#CONFIG_CAN_NPENDINGRTR +CONFIG_CAN_LOOPBACK=n +CONFIG_CAN1_BAUD=700000 +CONFIG_CAN2_BAUD=700000 + + +# XXX remove after integration testing +# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using +# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update +CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 +# Constant overhead for generating I2C start / stop conditions +CONFIG_STM32_I2CTIMEOUS_START_STOP=700 +# XXX this is bad and we want it gone +CONFIG_I2C_WRITEREAD=y + +# +# I2C configuration +# +CONFIG_I2C=y +CONFIG_I2C_POLLED=n +CONFIG_I2C_TRANSFER=y +CONFIG_I2C_TRACE=n +CONFIG_I2C_RESET=y +# XXX fixed per-transaction timeout +CONFIG_STM32_I2CTIMEOMS=10 + + +# XXX re-enable after integration testing + +# +# I2C configuration +# +#CONFIG_I2C=y +#CONFIG_I2C_POLLED=y +#CONFIG_I2C_TRANSFER=y +#CONFIG_I2C_TRACE=n +#CONFIG_I2C_RESET=y + +# Dynamic timeout +#CONFIG_STM32_I2C_DYNTIMEO=y +#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 +#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 + +# Fixed per-transaction timeout +#CONFIG_STM32_I2CTIMEOSEC=0 +#CONFIG_STM32_I2CTIMEOMS=10 + + + + + + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=y + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 192 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single +# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined +# then an additional, lower-priority work queue will also be created. This +# lower priority work queue is better suited for more extended processing +# (such as file system clean-up operations) +# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority +# worker thread. Default: 50 +# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread +# checks for work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower +# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="nsh_main" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=y +CONFIG_DEBUG_VERBOSE=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=2 +CONFIG_ARCH_LOWPUTC=y +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=y +CONFIG_SCHED_ATEXIT=n + +# +# System Logging +# +# CONFIG_SYSLOG - Enables the System Logging feature. +# CONFIG_RAMLOG - Enables the RAM logging feature +# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. +# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all +# console output will be re-directed to a circular buffer in RAM. This +# is useful, for example, if the only console is a Telnet console. Then +# in that case, console output from non-Telnet threads will go to the +# circular buffer and can be viewed using the NSH 'dmesg' command. +# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging +# interface. If this feature is enabled (along with CONFIG_SYSLOG), +# then all debug output (only) will be re-directed to the circular +# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' +# command. +# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting +# for this driver on poll(). Default: 4 +# +# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the +# following may also be provided: +# +# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 +# + +CONFIG_SYSLOG=n +CONFIG_RAMLOG=n +CONFIG_RAMLOG_CONSOLE=n +CONFIG_RAMLOG_SYSLOG=n +#CONFIG_RAMLOG_NPOLLWAITERS +#CONFIG_RAMLOG_CONSOLE_BUFSIZE + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=n +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=n +CONFIG_DISABLE_MOUNTPOINT=n +CONFIG_DISABLE_ENVIRON=n +CONFIG_DISABLE_POLL=n + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") +# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: +# 5.1234567 +# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) +# +CONFIG_NOPRINTF_FIELDWIDTH=n +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_HAVE_LONG_LONG=y + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=y +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=8 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Filesystem configuration +# +# CONFIG_FS_FAT - Enable FAT filesystem support +# CONFIG_FAT_SECTORSIZE - Max supported sector size +# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 +# file name support. +# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims +# patents on FAT long file name technology. Please read the +# disclaimer in the top-level COPYING file and only enable this +# feature if you understand these issues. +# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the +# default, maximum long file name is 255 bytes. This can eat up +# a lot of memory (especially stack space). If you are willing +# to live with some non-standard, short long file names, then +# define this value. A good choice would be the same value as +# selected for CONFIG_NAME_MAX which will limit the visibility +# of longer file names anyway. +# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. +# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. +# This must have one of the values of 0xff or 0x00. +# Default: 0xff. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. +# Default: 255. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# Default: 32. +# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean +# packing files together toward the end of the file or, if file are +# deleted at the end of the file, clean up can simply mean erasing +# the end of FLASH memory so that it can be re-used again. However, +# doing this can also harm the life of the FLASH part because it can +# mean that the tail end of the FLASH is re-used too often. This +# threshold determines if/when it is worth erased the tail end of FLASH +# and making it available for re-use (and possible over-wear). +# Default: 8192. +# CONFIG_FS_ROMFS - Enable ROMFS filesystem support +# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this +# option will enable a limited form of memory mapping that is +# implemented by copying whole files into memory. +# +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_FS_ROMFS=y +CONFIG_FS_BINFS=y + +# +# SPI-based MMC/SD driver +# +# CONFIG_MMCSD_NSLOTS +# Number of MMC/SD slots supported by the driver +# CONFIG_MMCSD_READONLY +# Provide read-only access (default is read/write) +# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. +# Default is 20MHz, current setting 24 MHz +# +CONFIG_MMCSD=n +# XXX need to rejig this for SDIO +#CONFIG_MMCSD_SPI=y +#CONFIG_MMCSD_NSLOTS=1 +#CONFIG_MMCSD_READONLY=n +#CONFIG_MMCSD_SPICLOCK=24000000 + +# +# Block driver buffering +# +# CONFIG_FS_READAHEAD +# Enable read-ahead buffering +# CONFIG_FS_WRITEBUFFER +# Enable write buffering +# +CONFIG_FS_READAHEAD=n +CONFIG_FS_WRITEBUFFER=n + +# +# RTC Configuration +# +# CONFIG_RTC - Enables general support for a hardware RTC. Specific +# architectures may require other specific settings. +# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple +# battery backed counter that keeps the time when power is down, and (2) +# A full date / time RTC the provides the date and time information, often +# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this +# second kind of RTC. In this case, the RTC is used to "seed" the normal +# NuttX timer and the NuttX system timer provides for higher resoution +# time. +# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, +# battery backed counter is used. There are two different implementations +# of such simple counters based on the time resolution of the counter: +# The typical RTC keeps time to resolution of 1 second, usually +# supporting a 32-bit time_t value. In this case, the RTC is used to +# "seed" the normal NuttX timer and the NuttX timer provides for higher +# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, +# then the RTC provides higher resolution time and completely replaces the +# system timer for purpose of date and time. +# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency +# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is +# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. +# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an +# alarm. A callback function will be executed when the alarm goes off +# +CONFIG_RTC=n +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HIRES=n +CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_ALARM=n + +# +# USB Device Configuration +# +# CONFIG_USBDEV +# Enables USB device support +# CONFIG_USBDEV_ISOCHRONOUS +# Build in extra support for isochronous endpoints +# CONFIG_USBDEV_DUALSPEED +# Hardware handles high and full speed operation (USB 2.0) +# CONFIG_USBDEV_SELFPOWERED +# Will cause USB features to indicate that the device is +# self-powered +# CONFIG_USBDEV_MAXPOWER +# Maximum power consumption in mA +# CONFIG_USBDEV_TRACE +# Enables USB tracing for debug +# CONFIG_USBDEV_TRACE_NRECORDS +# Number of trace entries to remember +# +CONFIG_USBDEV=y +CONFIG_USBDEV_ISOCHRONOUS=n +CONFIG_USBDEV_DUALSPEED=n +CONFIG_USBDEV_SELFPOWERED=y +CONFIG_USBDEV_REMOTEWAKEUP=n +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USBDEV_TRACE=n +CONFIG_USBDEV_TRACE_NRECORDS=512 + +# +# USB serial device class driver (Standard CDC ACM class) +# +# CONFIG_CDCACM +# Enable compilation of the USB serial driver +# CONFIG_CDCACM_CONSOLE +# Configures the CDC/ACM serial port as the console device. +# CONFIG_CDCACM_EP0MAXPACKET +# Endpoint 0 max packet size. Default 64 +# CONFIG_CDCACM_EPINTIN +# The logical 7-bit address of a hardware endpoint that supports +# interrupt IN operation. Default 2. +# CONFIG_CDCACM_EPINTIN_FSSIZE +# Max package size for the interrupt IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPINTIN_HSSIZE +# Max package size for the interrupt IN endpoint if high speed mode. +# Default 64 +# CONFIG_CDCACM_EPBULKOUT +# The logical 7-bit address of a hardware endpoint that supports +# bulk OUT operation. Default 4. +# CONFIG_CDCACM_EPBULKOUT_FSSIZE +# Max package size for the bulk OUT endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKOUT_HSSIZE +# Max package size for the bulk OUT endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_EPBULKIN +# The logical 7-bit address of a hardware endpoint that supports +# bulk IN operation. Default 3. +# CONFIG_CDCACM_EPBULKIN_FSSIZE +# Max package size for the bulk IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKIN_HSSIZE +# Max package size for the bulk IN endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS +# The number of write/read requests that can be in flight. +# Default 256. +# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR +# The vendor ID code/string. Default 0x0525 and "NuttX" +# 0x0525 is the Netchip vendor and should not be used in any +# products. This default VID was selected for compatibility with +# the Linux CDC ACM default VID. +# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR +# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" +# 0xa4a7 was selected for compatibility with the Linux CDC ACM +# default PID. +# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE +# Size of the serial receive/transmit buffers. Default 256. +# +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=n +#CONFIG_CDCACM_EP0MAXPACKET +CONFIG_CDCACM_EPINTIN=1 +#CONFIG_CDCACM_EPINTIN_FSSIZE +#CONFIG_CDCACM_EPINTIN_HSSIZE +CONFIG_CDCACM_EPBULKOUT=3 +#CONFIG_CDCACM_EPBULKOUT_FSSIZE +#CONFIG_CDCACM_EPBULKOUT_HSSIZE +CONFIG_CDCACM_EPBULKIN=2 +#CONFIG_CDCACM_EPBULKIN_FSSIZE +#CONFIG_CDCACM_EPBULKIN_HSSIZE +#CONFIG_CDCACM_NWRREQS +#CONFIG_CDCACM_NRDREQS +CONFIG_CDCACM_VENDORID=0x26AC +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" +#CONFIG_CDCACM_RXBUFSIZE +#CONFIG_CDCACM_TXBUFSIZE + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_BUILTIN=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAX_ARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_DISABLESCRIPT=n +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_USBCONSOLE=n +CONFIG_NSH_USBCONDEV="/dev/ttyACM0" +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_IOBUFFER_SIZE=512 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=y +CONFIG_NSH_IPADDR=0x0a000002 +CONFIG_NSH_DRIPADDR=0x0a000001 +CONFIG_NSH_NETMASK=0xffffff00 +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +#CONFIG_NSH_MMCSDSPIPORTNO=3 +#CONFIG_NSH_MMCSDSLOTNO=0 +#CONFIG_NSH_MMCSDMINOR=0 + + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3240G-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +# Idle thread needs 4096 bytes +# default 1 KB is not enough +# 4096 bytes +CONFIG_IDLETHREAD_STACKSIZE=6000 +# USERMAIN stack size probably needs to be around 4096 bytes +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= + +# enable bindir +CONFIG_APPS_BINDIR=y diff --git a/nuttx/configs/px4fmuv2/nsh/setenv.sh b/nuttx/configs/px4fmuv2/nsh/setenv.sh new file mode 100755 index 000000000..265520997 --- /dev/null +++ b/nuttx/configs/px4fmuv2/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4fmuv2/src/Makefile b/nuttx/configs/px4fmuv2/src/Makefile new file mode 100644 index 000000000..d4276f7fc --- /dev/null +++ b/nuttx/configs/px4fmuv2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx/configs/px4fmuv2/src/empty.c b/nuttx/configs/px4fmuv2/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx/configs/px4fmuv2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ -- cgit v1.2.3 From 976f1334efbb1218a0cd5af6e9f1d344b067eb13 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Apr 2013 23:17:21 -0700 Subject: More config for fmuv2 --- Makefile | 7 +++---- makefiles/board_px4fmu.mk | 2 +- makefiles/board_px4fmuv2.mk | 10 ++++++++++ makefiles/board_px4io.mk | 2 +- 4 files changed, 15 insertions(+), 6 deletions(-) create mode 100644 makefiles/board_px4fmuv2.mk diff --git a/Makefile b/Makefile index d0ffeb740..e87513e78 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,8 @@ include $(PX4_BASE)makefiles/setup.mk # # Canned firmware configurations that we build. # -CONFIGS ?= px4fmu_default px4io_default +CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +#CONFIGS ?= px4fmu_default px4io_default # # Boards that we build NuttX export kits for. @@ -162,9 +163,7 @@ help: @echo "" @echo " all" @echo " Build all firmware configs: $(CONFIGS)" - @echo " A limited set of configs can be built with:" - @echo "" - @echo " CONFIGS=" + @echo " A limited set of configs can be built with CONFIGS=" @echo "" @for config in $(CONFIGS); do \ echo " $$config"; \ diff --git a/makefiles/board_px4fmu.mk b/makefiles/board_px4fmu.mk index 959e4ed26..837069644 100644 --- a/makefiles/board_px4fmu.mk +++ b/makefiles/board_px4fmu.mk @@ -1,5 +1,5 @@ # -# Platform-specific definitions for the PX4FMU +# Board-specific definitions for the PX4FMU # # diff --git a/makefiles/board_px4fmuv2.mk b/makefiles/board_px4fmuv2.mk new file mode 100644 index 000000000..4b3b7e585 --- /dev/null +++ b/makefiles/board_px4fmuv2.mk @@ -0,0 +1,10 @@ +# +# Board-specific definitions for the PX4FMUv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4io.mk b/makefiles/board_px4io.mk index 275014aba..b0eb2dae7 100644 --- a/makefiles/board_px4io.mk +++ b/makefiles/board_px4io.mk @@ -1,5 +1,5 @@ # -# Platform-specific definitions for the PX4IO +# Board-specific definitions for the PX4IO # # -- cgit v1.2.3 From 2e5809d051121cbb73d92bf98be0a2952f1dbd2e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 00:04:59 -0700 Subject: Fix the remaining pieces so that we can build a firmware image for FMUv2 --- Images/px4fmuv2.prototype | 12 ++++++++++++ Makefile | 2 +- makefiles/config_px4fmuv2_default.mk | 6 ------ 3 files changed, 13 insertions(+), 7 deletions(-) create mode 100644 Images/px4fmuv2.prototype diff --git a/Images/px4fmuv2.prototype b/Images/px4fmuv2.prototype new file mode 100644 index 000000000..5109b77d1 --- /dev/null +++ b/Images/px4fmuv2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 9, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv2 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv2", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index e87513e78..0b8a65d94 100644 --- a/Makefile +++ b/Makefile @@ -48,7 +48,7 @@ CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config # # Boards that we build NuttX export kits for. # -BOARDS = px4fmu px4io +BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) # # Debugging diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d5bc758b8..2f104a5e4 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -22,7 +22,6 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ @@ -30,10 +29,8 @@ BUILTIN_COMMANDS := \ $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ $(call _B, delay_test, , 2048, delay_test_main ) \ - $(call _B, eeprom, , 4096, eeprom_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, fmu, , 2048, fmu_main ) \ $(call _B, gps, , 2048, gps_main ) \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ @@ -43,14 +40,11 @@ BUILTIN_COMMANDS := \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ $(call _B, mixer, , 4096, mixer_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 ) \ -- cgit v1.2.3 From 8b9b41fd507b9d2a62e2b0e3c5df398a489f7606 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 00:51:59 -0700 Subject: Populate INCLUDE_DIRS with some likely candidates. Implement __EXPORT and such for modules, as well as symbol visibility. Don't use UNZIP to point to unzip, as it looks there for arguments. --- makefiles/module.mk | 24 +++++++++++++++---- makefiles/nuttx.mk | 2 +- makefiles/setup.mk | 32 ++++++++++++++++--------- src/include/visibility.h | 62 ++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 103 insertions(+), 17 deletions(-) create mode 100644 src/include/visibility.h diff --git a/makefiles/module.mk b/makefiles/module.mk index 8b01d0a12..154d37cc2 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -75,6 +75,12 @@ # the list should be formatted as: # ... # +# DEFAULT_VISIBILITY (optional) +# +# If not set, global symbols defined in a module will not be visible +# outside the module. Symbols that should be globally visible must be +# marked __EXPORT. +# If set, global symbols defined in a module will be globally visible. # # @@ -98,11 +104,6 @@ $(error No module makefile specified) endif $(info % MODULE_MK = $(MODULE_MK)) -# -# Get path and tool config -# -include $(PX4_BASE)/makefiles/setup.mk - # # Get the board/toolchain config # @@ -150,6 +151,19 @@ $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) $(Q) $(TOUCH) $@ endif +################################################################################ +# Adjust compilation flags to implement EXPORT +################################################################################ + +ifeq ($(DEFAULT_VISIBILITY),) +DEFAULT_VISIBILITY = hidden +else +DEFAULT_VISIBILITY = default +endif + +CFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibility.h +CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibility.h + ################################################################################ # Build rules ################################################################################ diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index c6e2c86d1..e86f1370b 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -71,5 +71,5 @@ LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \ $(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE) @$(ECHO) %% Unpacking $(NUTTX_ARCHIVE) - $(Q) $(UNZIP) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) + $(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE) $(Q) $(TOUCH) $@ diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 8e7a00ef4..7655872e5 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -41,6 +41,7 @@ # the number of duplicate slashes we have lying around in paths, # and is consistent with joining the results of $(dir) and $(notdir). # +export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src/modules)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/nuttx)/ @@ -51,24 +52,33 @@ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ +# +# Default include paths +# +export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ + $(PX4_INCLUDE_DIR) + +# Include from legacy app/library path +export INCLUDE_DIRS += $(NUTTX_APP_SRC) + # # Tools # -MKFW = $(PX4_BASE)/Tools/px_mkfw.py -UPLOADER = $(PX4_BASE)/Tools/px_uploader.py -COPY = cp -REMOVE = rm -f -RMDIR = rm -rf -GENROMFS = genromfs -TOUCH = touch -MKDIR = mkdir -ECHO = echo -UNZIP = unzip +export MKFW = $(PX4_BASE)/Tools/px_mkfw.py +export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py +export COPY = cp +export REMOVE = rm -f +export RMDIR = rm -rf +export GENROMFS = genromfs +export TOUCH = touch +export MKDIR = mkdir +export ECHO = echo +export UNZIP_CMD = unzip # # Host-specific paths, hacks and fixups # -SYSTYPE := $(shell uname -s) +export SYSTYPE := $(shell uname -s) ifeq ($(SYSTYPE),Darwin) # Eclipse may not have the toolchain on its path. diff --git a/src/include/visibility.h b/src/include/visibility.h new file mode 100644 index 000000000..2c6adc062 --- /dev/null +++ b/src/include/visibility.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 visibility.h + * + * Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#ifndef __SYSTEMLIB_VISIBILITY_H +#define __SYSTEMLIB_VISIBILITY_H + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif +#endif \ No newline at end of file -- cgit v1.2.3 From c1f6f81e9d964f464cc474b36ce0b0b2935f1ab8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 00:56:26 -0700 Subject: wchar_t is indeed a builtin type that should not be overridden. --- makefiles/toolchain_gnu-arm-eabi.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d214006be..5e6a5bf04 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -157,6 +157,7 @@ CXXFLAGS = $(ARCHCXXFLAGS) \ $(INSTRUMENTATIONDEFINES) \ $(ARCHDEFINES) \ $(EXTRADEFINES) \ + -DCONFIG_WCHAR_BUILTIN \ $(addprefix -I,$(INCLUDE_DIRS)) # Flags we pass to the assembler -- cgit v1.2.3 From c558ad15abaab45ba1810f0f990f8ece87bed501 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 01:00:07 -0700 Subject: Add the RGB LED driver as an example. --- makefiles/config_px4fmuv2_default.mk | 5 + makefiles/setup.mk | 2 +- src/device/rgbled/module.mk | 6 + src/device/rgbled/rgbled.cpp | 490 +++++++++++++++++++++++++++++++++++ 4 files changed, 502 insertions(+), 1 deletion(-) create mode 100644 src/device/rgbled/module.mk create mode 100644 src/device/rgbled/rgbled.cpp diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 2f104a5e4..0ea0a9047 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -7,6 +7,11 @@ # ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) +# +# Board support modules +# +MODULES += device/rgbled + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 7655872e5..b798f7cab 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -42,7 +42,7 @@ # and is consistent with joining the results of $(dir) and $(notdir). # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ -export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src/modules)/ +export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/apps)/ diff --git a/src/device/rgbled/module.mk b/src/device/rgbled/module.mk new file mode 100644 index 000000000..39b53ec9e --- /dev/null +++ b/src/device/rgbled/module.mk @@ -0,0 +1,6 @@ +# +# TCA62724FMG driver for RGB LED +# + +MODULE_COMMAND = rgbled +SRCS = rgbled.cpp diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp new file mode 100644 index 000000000..c3b92ba7e --- /dev/null +++ b/src/device/rgbled/rgbled.cpp @@ -0,0 +1,490 @@ +/**************************************************************************** + * + * 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 rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + * + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#define LED_ONTIME 120 +#define LED_OFFTIME 120 + +#define RGBLED_DEVICE_PATH "/dev/rgbled" + +#define ADDR 0x55 /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +enum ledModes { + LED_MODE_TEST, + LED_MODE_SYSTEMSTATE, + LED_MODE_OFF +}; + +class RGBLED : public device::I2C +{ +public: + RGBLED(int bus, int rgbled); + virtual ~RGBLED(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int setMode(enum ledModes mode); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + enum ledColors { + LED_COLOR_OFF, + LED_COLOR_RED, + LED_COLOR_YELLOW, + LED_COLOR_PURPLE, + LED_COLOR_GREEN, + LED_COLOR_BLUE, + LED_COLOR_WHITE, + LED_COLOR_AMBER, + }; + + enum ledBlinkModes { + LED_BLINK_ON, + LED_BLINK_OFF + }; + + work_s _work; + + int led_colors[8]; + int led_blink; + + int mode; + int running; + + void setLEDColor(int ledcolor); + static void led_trampoline(void *arg); + void led(); + + int set(bool on, uint8_t r, uint8_t g, uint8_t b); + int set_on(bool on); + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); +}; + +/* for now, we only support one RGBLED */ +namespace +{ + RGBLED *g_rgbled; +} + + +extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); + +RGBLED::RGBLED(int bus, int rgbled) : + I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), + led_colors({0,0,0,0,0,0,0,0}), + led_blink(LED_BLINK_OFF), + mode(LED_MODE_OFF), + running(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +RGBLED::~RGBLED() +{ +} + +int +RGBLED::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + /* start off */ + set(false, 0, 0, 0); + + return OK; +} + +int +RGBLED::setMode(enum ledModes new_mode) +{ + switch (new_mode) { + case LED_MODE_SYSTEMSTATE: + case LED_MODE_TEST: + mode = new_mode; + if (!running) { + running = true; + set_on(true); + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + break; + case LED_MODE_OFF: + + default: + if (running) { + running = false; + set_on(false); + } + mode = LED_MODE_OFF; + break; + } + + return OK; +} + +int +RGBLED::probe() +{ + int ret; + bool on, not_powersave; + uint8_t r, g, b; + + ret = get(on, not_powersave, r, g, b); + + return ret; +} + +int +RGBLED::info() +{ + int ret; + bool on, not_powersave; + uint8_t r, g, b; + + ret = get(on, not_powersave, r, g, b); + + /* we don't care about power-save mode */ + log("State: %s", on ? "ON" : "OFF"); + log("Red: %d, Green: %d, Blue: %d", r, g, b); + + return ret; +} + +int +RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + + default: + break; + } + + return ret; +} + + +void +RGBLED::led_trampoline(void *arg) +{ + RGBLED *rgbl = (RGBLED *)arg; + + rgbl->led(); +} + + + +void +RGBLED::led() +{ + static int led_thread_runcount=0; + static int led_interval = 1000; + + switch (mode) { + case LED_MODE_TEST: + /* Demo LED pattern for now */ + led_colors[0] = LED_COLOR_YELLOW; + led_colors[1] = LED_COLOR_AMBER; + led_colors[2] = LED_COLOR_RED; + led_colors[3] = LED_COLOR_PURPLE; + led_colors[4] = LED_COLOR_BLUE; + led_colors[5] = LED_COLOR_GREEN; + led_colors[6] = LED_COLOR_WHITE; + led_colors[7] = LED_COLOR_OFF; + led_blink = LED_BLINK_ON; + break; + + case LED_MODE_SYSTEMSTATE: + /* XXX TODO set pattern */ + led_colors[0] = LED_COLOR_OFF; + led_colors[1] = LED_COLOR_OFF; + led_colors[2] = LED_COLOR_OFF; + led_colors[3] = LED_COLOR_OFF; + led_colors[4] = LED_COLOR_OFF; + led_colors[5] = LED_COLOR_OFF; + led_colors[6] = LED_COLOR_OFF; + led_colors[7] = LED_COLOR_OFF; + led_blink = LED_BLINK_OFF; + + break; + + case LED_MODE_OFF: + default: + return; + break; + } + + + if (led_thread_runcount & 1) { + if (led_blink == LED_BLINK_ON) + setLEDColor(LED_COLOR_OFF); + led_interval = LED_OFFTIME; + } else { + setLEDColor(led_colors[(led_thread_runcount/2) % 8]); + led_interval = LED_ONTIME; + } + + led_thread_runcount++; + + + if(running) { + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); + } else { + set_on(false); + } +} + +void RGBLED::setLEDColor(int ledcolor) { + switch (ledcolor) { + case LED_COLOR_OFF: // off + set_rgb(0,0,0); + break; + case LED_COLOR_RED: // red + set_rgb(255,0,0); + break; + case LED_COLOR_YELLOW: // yellow + set_rgb(255,70,0); + break; + case LED_COLOR_PURPLE: // purple + set_rgb(255,0,255); + break; + case LED_COLOR_GREEN: // green + set_rgb(0,255,0); + break; + case LED_COLOR_BLUE: // blue + set_rgb(0,0,255); + break; + case LED_COLOR_WHITE: // white + set_rgb(255,255,255); + break; + case LED_COLOR_AMBER: // amber + set_rgb(255,20,0); + break; + } +} + +int +RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) +{ + uint8_t settings_byte = 0; + + if (on) + settings_byte |= SETTING_ENABLE; +/* powersave not used */ +// if (not_powersave) + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::set_on(bool on) +{ + uint8_t settings_byte = 0; + + if (on) + settings_byte |= SETTING_ENABLE; + +/* powersave not used */ +// if (not_powersave) + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + + +int +RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2]; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + not_powersave = result[0] & SETTING_NOT_POWERSAVE; + r = (result[0] & 0x0f)*255/15; + g = (result[1] & 0xf0)*255/15; + b = (result[1] & 0x0f)*255/15; + } + + return ret; +} + + +void rgbled_usage() { + fprintf(stderr, "missing command: try 'start', 'systemstate', 'test', 'info', 'off'\n"); + fprintf(stderr, "options:\n"); + fprintf(stderr, "\t-b --bus i2cbus (3)\n"); + fprintf(stderr, "\t-a --ddr addr (9)\n"); +} + +int +rgbled_main(int argc, char *argv[]) +{ + + int i2cdevice = PX4_I2C_BUS_LED; + int rgbledadr = ADDR; /* 7bit */ + + int x; + + for (x = 1; x < argc; x++) { + if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { + if (argc > x + 1) { + i2cdevice = atoi(argv[x + 1]); + } + } + + if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) { + if (argc > x + 1) { + rgbledadr = atoi(argv[x + 1]); + } + } + + } + + if (!strcmp(argv[1], "start")) { + if (g_rgbled != nullptr) + errx(1, "already started"); + + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) + errx(1, "new failed"); + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } + + exit(0); + } + + + if (g_rgbled == nullptr) { + fprintf(stderr, "not started\n"); + rgbled_usage(); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + g_rgbled->setMode(LED_MODE_TEST); + exit(0); + } + + if (!strcmp(argv[1], "systemstate")) { + g_rgbled->setMode(LED_MODE_SYSTEMSTATE); + exit(0); + } + + if (!strcmp(argv[1], "info")) { + g_rgbled->info(); + exit(0); + } + + if (!strcmp(argv[1], "off")) { + g_rgbled->setMode(LED_MODE_OFF); + exit(0); + } + + + /* things that require access to the device */ + int fd = open(RGBLED_DEVICE_PATH, 0); + if (fd < 0) + err(1, "can't open RGBLED device"); + + rgbled_usage(); + exit(0); +} -- cgit v1.2.3 From 4ccf252b76cd4e52ecd11adffa63ee1e0b67ab37 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 3 Apr 2013 16:38:16 -0700 Subject: Changed I2C bus for blinkm, tested on fmuv2 --- apps/drivers/blinkm/blinkm.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 56265995f..3fabfd9a5 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -92,6 +92,7 @@ #include +#include #include #include @@ -841,7 +842,7 @@ int blinkm_main(int argc, char *argv[]) { - int i2cdevice = 3; + int i2cdevice = PX4_I2C_BUS_EXPANSION; int blinkmadr = 9; int x; -- cgit v1.2.3 From 4a5505b044a079d692ca631b6c3f268626ed2860 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Apr 2013 23:12:05 +0200 Subject: Added LSM303D driver skeleton --- apps/drivers/lsm303d/Makefile | 42 ++ apps/drivers/lsm303d/lsm303d.cpp | 833 +++++++++++++++++++++++++++++++++++ nuttx/configs/px4fmuv2/nsh/appconfig | 1 + 3 files changed, 876 insertions(+) create mode 100644 apps/drivers/lsm303d/Makefile create mode 100644 apps/drivers/lsm303d/lsm303d.cpp diff --git a/apps/drivers/lsm303d/Makefile b/apps/drivers/lsm303d/Makefile new file mode 100644 index 000000000..542a9bd40 --- /dev/null +++ b/apps/drivers/lsm303d/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the LSM303D driver. +# + +APPNAME = lsm303d +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp new file mode 100644 index 000000000..b0f900102 --- /dev/null +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -0,0 +1,833 @@ +/**************************************************************************** + * + * 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 lsm303d.cpp + * Driver for the ST LSM303D MEMS accel / mag connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +/* register addresses */ +#define ADDR_TEMP_OUT_L 0x05 +#define ADDR_TEMP_OUT_H 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x08 +#define ADDR_OUT_Y_H_M 0x09 +#define ADDR_OUT_Z_L_M 0x0A +#define ADDR_OUT_Z_H_M 0x0B + +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + +class LSM303D : public device::SPI +{ +public: + LSM303D(int bus, const char* path, spi_dev_e device); + virtual ~LSM303D(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct gyro_report *_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + orb_advert_t _gyro_topic; + + unsigned _current_rate; + unsigned _current_range; + + perf_counter_t _sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D measurement range. + * + * @param max_dps The measurement range is set to permit reading at least + * this rate in degrees per second. + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_dps); + + /** + * Set the LSM303D internal sampling frequency. + * + * @param frequency The internal sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + _call_interval(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _gyro_topic(-1), + _current_rate(0), + _current_range(0), + _sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_read")) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; +} + +LSM303D::~LSM303D() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; + + /* delete the perf counter */ + perf_free(_sample_perf); +} + +int +LSM303D::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _oldest_report = _next_report = 0; + _reports = new struct accel_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + /* advertise sensor topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + +// /* set default configuration */ +// write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); +// write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ +// write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ +// write_reg(ADDR_CTRL_REG4, REG4_BDU); +// write_reg(ADDR_CTRL_REG5, 0); +// +// write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ +// write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + set_range(500); /* default to 500dps */ + set_samplerate(0); /* max sample rate */ + + ret = OK; +out: + return ret; +} + +int +LSM303D::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +LSM303D::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct gyro_report); + int ret = 0; + +// /* buffer must be large enough */ +// if (count < 1) +// return -ENOSPC; +// +// /* if automatic measurement is enabled */ +// if (_call_interval > 0) { +// +// /* +// * While there is space in the caller's buffer, and reports, copy them. +// * Note that we may be pre-empted by the measurement code while we are doing this; +// * we are careful to avoid racing with it. +// */ +// while (count--) { +// if (_oldest_report != _next_report) { +// memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); +// ret += sizeof(_reports[0]); +// INCREMENT(_oldest_report, _num_reports); +// } +// } +// +// /* if there was no data, warn the caller */ +// return ret ? ret : -EAGAIN; +// } +// +// /* manual measurement */ +// _oldest_report = _next_report = 0; +// measure(); +// +// /* measurement will have generated a report, copy it out */ +// memcpy(buffer, _reports, sizeof(*_reports)); +// ret = sizeof(*_reports); + + return ret; +} + +int +LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + +// case SENSORIOCSPOLLRATE: { +// switch (arg) { +// +// /* switching to manual polling */ +// case SENSOR_POLLRATE_MANUAL: +// stop(); +// _call_interval = 0; +// return OK; +// +// /* external signalling not supported */ +// case SENSOR_POLLRATE_EXTERNAL: +// +// /* zero would be bad */ +// case 0: +// return -EINVAL; +// +// /* set default/max polling rate */ +// case SENSOR_POLLRATE_MAX: +// case SENSOR_POLLRATE_DEFAULT: +// /* With internal low pass filters enabled, 250 Hz is sufficient */ +// return ioctl(filp, SENSORIOCSPOLLRATE, 250); +// +// /* adjust to a legal polling interval in Hz */ +// default: { +// /* do we need to start internal polling? */ +// bool want_start = (_call_interval == 0); +// +// /* convert hz to hrt interval via microseconds */ +// unsigned ticks = 1000000 / arg; +// +// /* check against maximum sane rate */ +// if (ticks < 1000) +// return -EINVAL; +// +// /* update interval for next measurement */ +// /* XXX this is a bit shady, but no other way to adjust... */ +// _call.period = _call_interval = ticks; +// +// /* if we need to start the poll state machine, do it */ +// if (want_start) +// start(); +// +// return OK; +// } +// } +// } +// +// case SENSORIOCGPOLLRATE: +// if (_call_interval == 0) +// return SENSOR_POLLRATE_MANUAL; +// +// return 1000000 / _call_interval; +// +// case SENSORIOCSQUEUEDEPTH: { +// /* account for sentinel in the ring */ +// arg++; +// +// /* lower bound is mandatory, upper bound is a sanity check */ +// if ((arg < 2) || (arg > 100)) +// return -EINVAL; +// +// /* allocate new buffer */ +// struct gyro_report *buf = new struct gyro_report[arg]; +// +// if (nullptr == buf) +// return -ENOMEM; +// +// /* reset the measurement state machine with the new buffer, free the old */ +// stop(); +// delete[] _reports; +// _num_reports = arg; +// _reports = buf; +// start(); +// +// return OK; +// } +// +// case SENSORIOCGQUEUEDEPTH: +// return _num_reports - 1; +// +// case SENSORIOCRESET: +// /* XXX implement */ +// return -EINVAL; +// +// case GYROIOCSSAMPLERATE: +// return set_samplerate(arg); +// +// case GYROIOCGSAMPLERATE: +// return _current_rate; +// +// case GYROIOCSLOWPASS: +// case GYROIOCGLOWPASS: +// /* XXX not implemented due to wacky interaction with samplerate */ +// return -EINVAL; +// +// case GYROIOCSSCALE: +// /* copy scale in */ +// memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); +// return OK; +// +// case GYROIOCGSCALE: +// /* copy scale out */ +// memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); +// return OK; +// +// case GYROIOCSRANGE: +// return set_range(arg); +// +// case GYROIOCGRANGE: +// return _current_range; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +LSM303D::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +LSM303D::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +LSM303D::set_range(unsigned max_dps) +{ +// uint8_t bits = REG4_BDU; +// +// if (max_dps == 0) +// max_dps = 2000; +// +// if (max_dps <= 250) { +// _current_range = 250; +// bits |= RANGE_250DPS; +// +// } else if (max_dps <= 500) { +// _current_range = 500; +// bits |= RANGE_500DPS; +// +// } else if (max_dps <= 2000) { +// _current_range = 2000; +// bits |= RANGE_2000DPS; +// +// } else { +// return -EINVAL; +// } +// +// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; +// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; +// write_reg(ADDR_CTRL_REG4, bits); + + return OK; +} + +int +LSM303D::set_samplerate(unsigned frequency) +{ +// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; +// +// if (frequency == 0) +// frequency = 760; +// +// if (frequency <= 95) { +// _current_rate = 95; +// bits |= RATE_95HZ_LP_25HZ; +// +// } else if (frequency <= 190) { +// _current_rate = 190; +// bits |= RATE_190HZ_LP_25HZ; +// +// } else if (frequency <= 380) { +// _current_rate = 380; +// bits |= RATE_380HZ_LP_30HZ; +// +// } else if (frequency <= 760) { +// _current_rate = 760; +// bits |= RATE_760HZ_LP_30HZ; +// +// } else { +// return -EINVAL; +// } +// +// write_reg(ADDR_CTRL_REG1, bits); + + return OK; +} + +void +LSM303D::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&LSM303D::measure_trampoline, this); +} + +void +LSM303D::stop() +{ + hrt_cancel(&_call); +} + +void +LSM303D::measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +LSM303D::measure() +{ +// /* status register and data as read back from the device */ +//#pragma pack(push, 1) +// struct { +// uint8_t cmd; +// uint8_t temp; +// uint8_t status; +// int16_t x; +// int16_t y; +// int16_t z; +// } raw_report; +//#pragma pack(pop) +// +// gyro_report *report = &_reports[_next_report]; +// +// /* start the performance counter */ +// perf_begin(_sample_perf); +// +// /* fetch data from the sensor */ +// raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; +// transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +// +// /* +// * 1) Scale raw value to SI units using scaling from datasheet. +// * 2) Subtract static offset (in SI units) +// * 3) Scale the statically calibrated values with a linear +// * dynamically obtained factor +// * +// * Note: the static sensor offset is the number the sensor outputs +// * at a nominally 'zero' input. Therefore the offset has to +// * be subtracted. +// * +// * Example: A gyro outputs a value of 74 at zero angular rate +// * the offset is 74 from the origin and subtracting +// * 74 from all measurements centers them around zero. +// */ +// report->timestamp = hrt_absolute_time(); +// /* XXX adjust for sensor alignment to board here */ +// report->x_raw = raw_report.x; +// report->y_raw = raw_report.y; +// report->z_raw = raw_report.z; +// +// report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; +// report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; +// report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; +// +// /* post a report to the ring - note, not locked */ +// INCREMENT(_next_report, _num_reports); +// +// /* if we are running up against the oldest report, fix it */ +// if (_next_report == _oldest_report) +// INCREMENT(_oldest_report, _num_reports); +// +// /* notify anyone waiting for data */ +// poll_notify(POLLIN); +// +// /* publish for subscribers */ +// orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + + /* stop the perf counter */ + perf_end(_sample_perf); +} + +void +LSM303D::print_info() +{ + perf_print_counter(_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_accel = -1; + struct accel_report a_report; + ssize_t sz; + + /* get the driver */ + fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd_accel < 0) + err(1, "%s open failed", ACCEL_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd_accel, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) + err(1, "immediate gyro read failed"); + + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); + warnx("accel x: \t%d\traw", (int)a_report.x_raw); + warnx("accel y: \t%d\traw", (int)a_report.y_raw); + warnx("accel z: \t%d\traw", (int)a_report.z_raw); + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + LSM303D::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + lsm303d::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + lsm303d::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + lsm303d::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fbcb67dcf..fff140673 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -116,6 +116,7 @@ CONFIGURED_APPS += drivers/device # XXX needs conversion to SPI #CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/l3gd20 +CONFIGURED_APPS += drivers/lsm303d # XXX needs conversion to serial #CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 -- cgit v1.2.3 From b4483a09b2886a6c0ecd78703e1f06e776d3a6d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Apr 2013 23:22:39 +0200 Subject: Added LSM303D driver --- apps/drivers/lsm303d/lsm303d.cpp | 48 ++++++++++++++++++++-------------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index b0f900102..c00726b4e 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -123,12 +123,12 @@ private: unsigned _num_reports; volatile unsigned _next_report; volatile unsigned _oldest_report; - struct gyro_report *_reports; + struct accel_report *_reports; - struct gyro_scale _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; - orb_advert_t _gyro_topic; + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; unsigned _current_rate; unsigned _current_range; @@ -220,9 +220,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_report(0), _oldest_report(0), _reports(nullptr), - _gyro_range_scale(0.0f), - _gyro_range_rad_s(0.0f), - _gyro_topic(-1), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), _current_rate(0), _current_range(0), _sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_read")) @@ -231,12 +231,12 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _debug_enabled = true; // default scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; } LSM303D::~LSM303D() @@ -307,7 +307,7 @@ LSM303D::probe() ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct gyro_report); + unsigned count = buflen / sizeof(struct accel_report); int ret = 0; // /* buffer must be large enough */ @@ -412,7 +412,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) // return -EINVAL; // // /* allocate new buffer */ -// struct gyro_report *buf = new struct gyro_report[arg]; +// struct accel_report *buf = new struct accel_report[arg]; // // if (nullptr == buf) // return -ENOMEM; @@ -447,12 +447,12 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) // // case GYROIOCSSCALE: // /* copy scale in */ -// memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); +// memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); // return OK; // // case GYROIOCGSCALE: // /* copy scale out */ -// memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); +// memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); // return OK; // // case GYROIOCSRANGE: @@ -608,7 +608,7 @@ LSM303D::measure() // } raw_report; //#pragma pack(pop) // -// gyro_report *report = &_reports[_next_report]; +// accel_report *report = &_reports[_next_report]; // // /* start the performance counter */ // perf_begin(_sample_perf); @@ -637,9 +637,9 @@ LSM303D::measure() // report->y_raw = raw_report.y; // report->z_raw = raw_report.z; // -// report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -// report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; -// report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; +// report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; +// report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; +// report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; // report->scaling = _gyro_range_scale; // report->range_rad_s = _gyro_range_rad_s; // @@ -693,7 +693,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL); + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) goto fail; @@ -809,7 +809,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) - LSM303D::start(); + lsm303d::start(); /* * Test the driver/device. -- cgit v1.2.3 From 2187dc8e9abced1ab88e08feaf24e026f728ea5c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 4 Apr 2013 19:46:55 -0700 Subject: LSM303D accel raw values look ok (work in progress) --- apps/drivers/lsm303d/lsm303d.cpp | 405 ++++++++++++++++++++------------------- 1 file changed, 206 insertions(+), 199 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index c00726b4e..b10c39f8f 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -83,10 +83,31 @@ static const int ERROR = -1; #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x08 -#define ADDR_OUT_Y_H_M 0x09 -#define ADDR_OUT_Z_L_M 0x0A -#define ADDR_OUT_Z_H_M 0x0B +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_OUT_TEMP_A 0x26 +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG1 0x20 + +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) + +#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0x49 @@ -274,7 +295,7 @@ LSM303D::init() _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); // /* set default configuration */ -// write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG1, REG1_RATE_100HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); // write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ // write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ // write_reg(ADDR_CTRL_REG4, REG4_BDU); @@ -310,37 +331,37 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(struct accel_report); int ret = 0; -// /* buffer must be large enough */ -// if (count < 1) -// return -ENOSPC; -// -// /* if automatic measurement is enabled */ -// if (_call_interval > 0) { -// -// /* -// * While there is space in the caller's buffer, and reports, copy them. -// * Note that we may be pre-empted by the measurement code while we are doing this; -// * we are careful to avoid racing with it. -// */ -// while (count--) { -// if (_oldest_report != _next_report) { -// memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); -// ret += sizeof(_reports[0]); -// INCREMENT(_oldest_report, _num_reports); -// } -// } -// -// /* if there was no data, warn the caller */ -// return ret ? ret : -EAGAIN; -// } -// -// /* manual measurement */ -// _oldest_report = _next_report = 0; -// measure(); -// -// /* measurement will have generated a report, copy it out */ -// memcpy(buffer, _reports, sizeof(*_reports)); -// ret = sizeof(*_reports); + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -350,116 +371,89 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { -// case SENSORIOCSPOLLRATE: { -// switch (arg) { -// -// /* switching to manual polling */ -// case SENSOR_POLLRATE_MANUAL: -// stop(); -// _call_interval = 0; -// return OK; -// -// /* external signalling not supported */ -// case SENSOR_POLLRATE_EXTERNAL: -// -// /* zero would be bad */ -// case 0: -// return -EINVAL; -// -// /* set default/max polling rate */ -// case SENSOR_POLLRATE_MAX: -// case SENSOR_POLLRATE_DEFAULT: -// /* With internal low pass filters enabled, 250 Hz is sufficient */ -// return ioctl(filp, SENSORIOCSPOLLRATE, 250); -// -// /* adjust to a legal polling interval in Hz */ -// default: { -// /* do we need to start internal polling? */ -// bool want_start = (_call_interval == 0); -// -// /* convert hz to hrt interval via microseconds */ -// unsigned ticks = 1000000 / arg; -// -// /* check against maximum sane rate */ -// if (ticks < 1000) -// return -EINVAL; -// -// /* update interval for next measurement */ -// /* XXX this is a bit shady, but no other way to adjust... */ -// _call.period = _call_interval = ticks; -// -// /* if we need to start the poll state machine, do it */ -// if (want_start) -// start(); -// -// return OK; -// } -// } -// } -// -// case SENSORIOCGPOLLRATE: -// if (_call_interval == 0) -// return SENSOR_POLLRATE_MANUAL; -// -// return 1000000 / _call_interval; -// -// case SENSORIOCSQUEUEDEPTH: { -// /* account for sentinel in the ring */ -// arg++; -// -// /* lower bound is mandatory, upper bound is a sanity check */ -// if ((arg < 2) || (arg > 100)) -// return -EINVAL; -// -// /* allocate new buffer */ -// struct accel_report *buf = new struct accel_report[arg]; -// -// if (nullptr == buf) -// return -ENOMEM; -// -// /* reset the measurement state machine with the new buffer, free the old */ -// stop(); -// delete[] _reports; -// _num_reports = arg; -// _reports = buf; -// start(); -// -// return OK; -// } -// -// case SENSORIOCGQUEUEDEPTH: -// return _num_reports - 1; -// -// case SENSORIOCRESET: -// /* XXX implement */ -// return -EINVAL; -// -// case GYROIOCSSAMPLERATE: -// return set_samplerate(arg); -// -// case GYROIOCGSAMPLERATE: -// return _current_rate; -// -// case GYROIOCSLOWPASS: -// case GYROIOCGLOWPASS: -// /* XXX not implemented due to wacky interaction with samplerate */ -// return -EINVAL; -// -// case GYROIOCSSCALE: -// /* copy scale in */ -// memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale)); -// return OK; -// -// case GYROIOCGSCALE: -// /* copy scale out */ -// memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); -// return OK; -// -// case GYROIOCSRANGE: -// return set_range(arg); -// -// case GYROIOCGRANGE: -// return _current_range; + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; default: /* give it to the superclass */ @@ -596,65 +590,78 @@ LSM303D::measure_trampoline(void *arg) void LSM303D::measure() { -// /* status register and data as read back from the device */ + /* status register and data as read back from the device */ //#pragma pack(push, 1) // struct { // uint8_t cmd; -// uint8_t temp; +// uint16_t temp; // uint8_t status; // int16_t x; // int16_t y; // int16_t z; -// } raw_report; +// } raw_report_mag; //#pragma pack(pop) -// -// accel_report *report = &_reports[_next_report]; -// -// /* start the performance counter */ -// perf_begin(_sample_perf); -// -// /* fetch data from the sensor */ -// raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; -// transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -// -// /* -// * 1) Scale raw value to SI units using scaling from datasheet. -// * 2) Subtract static offset (in SI units) -// * 3) Scale the statically calibrated values with a linear -// * dynamically obtained factor -// * -// * Note: the static sensor offset is the number the sensor outputs -// * at a nominally 'zero' input. Therefore the offset has to -// * be subtracted. -// * -// * Example: A gyro outputs a value of 74 at zero angular rate -// * the offset is 74 from the origin and subtracting -// * 74 from all measurements centers them around zero. -// */ -// report->timestamp = hrt_absolute_time(); -// /* XXX adjust for sensor alignment to board here */ -// report->x_raw = raw_report.x; -// report->y_raw = raw_report.y; -// report->z_raw = raw_report.z; -// + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_report_accel; +#pragma pack(pop) + + accel_report *report = &_reports[_next_report]; + + /* start the performance counter */ + perf_begin(_sample_perf); + + /* fetch data from the sensor */ + raw_report_accel.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_report_accel, (uint8_t *)&raw_report_accel, sizeof(raw_report_accel)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + report->x_raw = raw_report_accel.x; + report->y_raw = raw_report_accel.y; + report->z_raw = raw_report_accel.z; + + // report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; // report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; // report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; // report->scaling = _gyro_range_scale; // report->range_rad_s = _gyro_range_rad_s; -// -// /* post a report to the ring - note, not locked */ -// INCREMENT(_next_report, _num_reports); -// -// /* if we are running up against the oldest report, fix it */ -// if (_next_report == _oldest_report) -// INCREMENT(_oldest_report, _num_reports); -// -// /* notify anyone waiting for data */ -// poll_notify(POLLIN); -// -// /* publish for subscribers */ -// orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_report == _oldest_report) + INCREMENT(_oldest_report, _num_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, report); /* stop the perf counter */ perf_end(_sample_perf); @@ -740,22 +747,22 @@ test() err(1, "%s open failed", ACCEL_DEVICE_PATH); /* reset to manual polling */ - if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) - err(1, "reset to manual polling"); +// if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) +// err(1, "reset to manual polling"); /* do a simple demand read */ sz = read(fd_accel, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) - err(1, "immediate gyro read failed"); + err(1, "immediate read failed"); - warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); - warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); - warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); +// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); +// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); +// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); +// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); /* XXX add poll-rate tests here too */ -- cgit v1.2.3 From f288c65baa9d75d657bebf5029f557c7c0d8044c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 5 Apr 2013 16:36:42 -0700 Subject: LSM303D accel and mag working (still WIP) --- apps/drivers/lsm303d/lsm303d.cpp | 562 +++++++++++++++++++++++++++++++++------ 1 file changed, 488 insertions(+), 74 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index b10c39f8f..49f2f29ee 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -98,17 +98,30 @@ static const int ERROR = -1; #define ADDR_OUT_Z_H_A 0x2D #define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG7 0x26 #define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) #define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define REG1_CONT_UPDATE_A (0<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) +#define REG5_TEMP_ENABLE (1<<7) +#define REG5_M_RES_HIGH ((1<<6) | (1<<5)) + +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) + + +#define REG7_CONTINUOUS_MODE_M ((0<<1) | (0<<0)) + #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0x49 @@ -117,6 +130,8 @@ static const int ERROR = -1; extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } +class LSM303D_mag; + class LSM303D : public device::SPI { public: @@ -136,25 +151,49 @@ public: protected: virtual int probe(); + friend class LSM303D_mag; + + virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + private: - struct hrt_call _call; - unsigned _call_interval; + LSM303D_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - unsigned _current_rate; - unsigned _current_range; + unsigned _num_mag_reports; + volatile unsigned _next_mag_report; + volatile unsigned _oldest_mag_report; + struct mag_report *_mag_reports; + + struct mag_scale _mag_scale; + float _mag_range_scale; + float _mag_range_ga; + orb_advert_t _mag_topic; + + unsigned _current_accel_rate; + unsigned _current_accel_range; + + unsigned _current_mag_rate; + unsigned _current_mag_range; - perf_counter_t _sample_perf; + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; /** * Start automatic measurement. @@ -177,11 +216,15 @@ private: */ static void measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); + /** * Fetch measurements from the sensor and update the report ring. */ void measure(); + void mag_measure(); + /** * Read a register from the LSM303D * @@ -230,27 +273,66 @@ private: int set_samplerate(unsigned frequency); }; +/** + * Helper class implementing the mag driver node. + */ +class LSM303D_mag : public device::CDev +{ +public: + LSM303D_mag(LSM303D *parent); + ~LSM303D_mag(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class LSM303D; + + void parent_poll_notify(); +private: + LSM303D *_parent; + + void measure(); + + void measure_trampoline(void *arg); +}; + + /* helper macro for handling report buffer indices */ #define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), - _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), + _mag(new LSM303D_mag(this)), + _call_accel_interval(0), + _call_mag_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _current_rate(0), - _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_read")) + _num_mag_reports(0), + _next_mag_report(0), + _oldest_mag_report(0), + _mag_reports(nullptr), + _mag_range_scale(0.0f), + _mag_range_ga(0.0f), + _current_accel_rate(0), + _current_accel_range(0), + _current_mag_rate(0), + _current_mag_range(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) { // enable debug() calls _debug_enabled = true; + _accel_range_scale = 1.0f; + _mag_range_scale = 1.0f; + // default scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -258,6 +340,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0; + _mag_scale.z_scale = 1.0f; } LSM303D::~LSM303D() @@ -266,36 +355,57 @@ LSM303D::~LSM303D() stop(); /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; + if (_accel_reports != nullptr) + delete[] _accel_reports; + if (_mag_reports != nullptr) + delete[] _mag_reports; + + delete _mag; /* delete the perf counter */ - perf_free(_sample_perf); + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); } int LSM303D::init() { int ret = ERROR; + int mag_ret; + int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; + + if (_accel_reports == nullptr) + goto out; + + /* advertise accel topic */ + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + + _num_mag_reports = 2; + _oldest_mag_report = _next_mag_report = 0; + _mag_reports = new struct mag_report[_num_mag_reports]; - if (_reports == nullptr) + if (_mag_reports == nullptr) goto out; - /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + /* advertise mag topic */ + memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + write_reg(ADDR_CTRL_REG7, REG7_CONTINUOUS_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_TEMP_ENABLE | REG5_RATE_50HZ_M | REG5_M_RES_HIGH); -// /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_RATE_100HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); // write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ // write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ // write_reg(ADDR_CTRL_REG4, REG4_BDU); @@ -307,6 +417,15 @@ LSM303D::init() set_range(500); /* default to 500dps */ set_samplerate(0); /* max sample rate */ +// _current_accel_rate = 100; + + /* do CDev init for the gyro device node, keep it optional */ + mag_ret = _mag->init(); + + if (mag_ret != OK) { + _mag_topic = -1; + } + ret = OK; out: return ret; @@ -336,7 +455,48 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_call_interval > 0) { + if (_call_accel_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_accel_report = _next_accel_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); + + return ret; +} + +ssize_t +LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -344,10 +504,10 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_oldest_mag_report != _next_mag_report) { + memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); + ret += sizeof(_mag_reports[0]); + INCREMENT(_oldest_mag_report, _num_mag_reports); } } @@ -356,12 +516,12 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _oldest_mag_report = _next_mag_report = 0; measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); + ret = sizeof(*_mag_reports); return ret; } @@ -377,7 +537,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); - _call_interval = 0; + _call_accel_interval = 0; return OK; /* external signalling not supported */ @@ -396,7 +556,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); + bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; @@ -407,7 +567,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; + _accel_call.period = _call_accel_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) @@ -419,10 +579,10 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_accel_interval == 0) return SENSOR_POLLRATE_MANUAL; - return 1000000 / _call_interval; + return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { /* account for sentinel in the ring */ @@ -440,16 +600,16 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* reset the measurement state machine with the new buffer, free the old */ stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; start(); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _num_accel_reports - 1; case SENSORIOCRESET: /* XXX implement */ @@ -461,6 +621,110 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 50 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + case SENSORIOCSQUEUEDEPTH: + case SENSORIOCGQUEUEDEPTH: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case MAGIOCSSAMPLERATE: +// case MAGIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case MAGIOCSLOWPASS: +// case MAGIOCGLOWPASS: + /* XXX not implemented */ +// _set_dlpf_filter((uint16_t)arg); + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: +// case MAGIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _mag_range_scale = xx + // _mag_range_ga = xx + return -EINVAL; + + case MAGIOCSELFTEST: + /* XXX not implemented */ +// return self_test(); + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + uint8_t LSM303D::read_reg(unsigned reg) { @@ -529,6 +793,8 @@ LSM303D::set_range(unsigned max_dps) int LSM303D::set_samplerate(unsigned frequency) { + _current_accel_rate = 100; + // uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; // // if (frequency == 0) @@ -566,16 +832,19 @@ LSM303D::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _oldest_accel_report = _next_accel_report = 0; + _oldest_mag_report = _next_mag_report = 0; /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } void LSM303D::stop() { - hrt_cancel(&_call); + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); } void @@ -587,6 +856,15 @@ LSM303D::measure_trampoline(void *arg) dev->measure(); } +void +LSM303D::mag_measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + void LSM303D::measure() { @@ -609,17 +887,17 @@ LSM303D::measure() int16_t x; int16_t y; int16_t z; - } raw_report_accel; + } raw_accel_report; #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + accel_report *accel_report = &_accel_reports[_next_accel_report]; /* start the performance counter */ - perf_begin(_sample_perf); + perf_begin(_accel_sample_perf); /* fetch data from the sensor */ - raw_report_accel.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_report_accel, (uint8_t *)&raw_report_accel, sizeof(raw_report_accel)); + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -637,42 +915,151 @@ LSM303D::measure() */ - report->timestamp = hrt_absolute_time(); + accel_report->timestamp = hrt_absolute_time(); /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report_accel.x; - report->y_raw = raw_report_accel.y; - report->z_raw = raw_report_accel.z; + accel_report->x_raw = raw_accel_report.x; + accel_report->y_raw = raw_accel_report.y; + accel_report->z_raw = raw_accel_report.z; + + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_accel_report, _num_accel_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +LSM303D::mag_measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report *mag_report = &_mag_reports[_next_mag_report]; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ -// report->x = ((report->x_raw * _gyro_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; -// report->y = ((report->y_raw * _gyro_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; -// report->z = ((report->z_raw * _gyro_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + mag_report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + mag_report->x_raw = raw_mag_report.x; + mag_report->y_raw = raw_mag_report.y; + mag_report->z_raw = raw_mag_report.z; + mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; // report->scaling = _gyro_range_scale; // report->range_rad_s = _gyro_range_rad_s; /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + INCREMENT(_next_mag_report, _num_mag_reports); /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + if (_next_mag_report == _oldest_mag_report) + INCREMENT(_oldest_mag_report, _num_mag_reports); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); /* stop the perf counter */ - perf_end(_sample_perf); + perf_end(_mag_sample_perf); } void LSM303D::print_info() { - perf_print_counter(_sample_perf); + perf_print_counter(_accel_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); + perf_print_counter(_mag_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); +} + +LSM303D_mag::LSM303D_mag(LSM303D *parent) : + CDev("LSM303D_mag", MAG_DEVICE_PATH), + _parent(parent) +{ +} + +LSM303D_mag::~LSM303D_mag() +{ +} + +void +LSM303D_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->mag_ioctl(filp, cmd, arg); +} + +void +LSM303D_mag::measure() +{ + _parent->mag_measure(); +} + +void +LSM303D_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); } /** @@ -694,7 +1081,7 @@ void info(); void start() { - int fd; + int fd, fd_mag; if (g_dev != nullptr) errx(1, "already started"); @@ -717,6 +1104,16 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + } + + exit(0); fail: @@ -746,10 +1143,6 @@ test() if (fd_accel < 0) err(1, "%s open failed", ACCEL_DEVICE_PATH); - /* reset to manual polling */ -// if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) -// err(1, "reset to manual polling"); - /* do a simple demand read */ sz = read(fd_accel, &a_report, sizeof(a_report)); @@ -764,9 +1157,30 @@ test() warnx("accel z: \t%d\traw", (int)a_report.z_raw); // warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + + + int fd_mag = -1; + struct mag_report m_report; + + /* get the driver */ + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) + err(1, "%s open failed", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_mag, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) + err(1, "immediate read failed"); + + warnx("mag x: \t%d\traw", (int)m_report.x_raw); + warnx("mag y: \t%d\traw", (int)m_report.y_raw); + warnx("mag z: \t%d\traw", (int)m_report.z_raw); + /* XXX add poll-rate tests here too */ - reset(); +// reset(); errx(0, "PASS"); } -- cgit v1.2.3 From d1d4d1d1e2f29ef540caec51d1a6b1244437d756 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 5 Apr 2013 17:31:53 -0700 Subject: Some defines and comments (still WIP) --- apps/drivers/lsm303d/lsm303d.cpp | 114 +++++++++++++++++++++++++-------------- 1 file changed, 75 insertions(+), 39 deletions(-) diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp index 49f2f29ee..32030a1f7 100644 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ b/apps/drivers/lsm303d/lsm303d.cpp @@ -33,7 +33,7 @@ /** * @file lsm303d.cpp - * Driver for the ST LSM303D MEMS accel / mag connected via SPI. + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. */ #include @@ -75,11 +75,16 @@ static const int ERROR = -1; /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) +#define ADDR_INCREMENT (1<<6) -/* register addresses */ -#define ADDR_TEMP_OUT_L 0x05 -#define ADDR_TEMP_OUT_H 0x06 + + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_L_T 0x05 +#define ADDR_OUT_H_T 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 @@ -97,39 +102,70 @@ static const int ERROR = -1; #define ADDR_OUT_Z_L_A 0x2C #define ADDR_OUT_Z_H_A 0x2D +#define ADDR_CTRL_REG0 0x1F #define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 #define ADDR_CTRL_REG5 0x24 #define ADDR_CTRL_REG7 0x26 -#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define REG1_CONT_UPDATE_A (0<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) -#define REG5_TEMP_ENABLE (1<<7) -#define REG5_M_RES_HIGH ((1<<6) | (1<<5)) +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) #define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) +#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) +#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) +#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) +#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) -#define REG7_CONTINUOUS_MODE_M ((0<<1) | (0<<0)) +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + class LSM303D_mag; class LSM303D : public device::SPI @@ -216,13 +252,21 @@ private: */ static void measure_trampoline(void *arg); + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ static void mag_measure_trampoline(void *arg); /** - * Fetch measurements from the sensor and update the report ring. + * Fetch accel measurements from the sensor and update the report ring. */ void measure(); + /** + * Fetch mag measurements from the sensor and update the report ring. + */ void mag_measure(); /** @@ -330,6 +374,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : // enable debug() calls _debug_enabled = true; + /* XXX fix this default values */ _accel_range_scale = 1.0f; _mag_range_scale = 1.0f; @@ -401,25 +446,21 @@ LSM303D::init() memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + /* XXX do this with ioctls */ /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); - write_reg(ADDR_CTRL_REG7, REG7_CONTINUOUS_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_TEMP_ENABLE | REG5_RATE_50HZ_M | REG5_M_RES_HIGH); + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); -// write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ -// write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ -// write_reg(ADDR_CTRL_REG4, REG4_BDU); -// write_reg(ADDR_CTRL_REG5, 0); -// -// write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ -// write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + /* XXX should we enable FIFO */ set_range(500); /* default to 500dps */ set_samplerate(0); /* max sample rate */ // _current_accel_rate = 100; - /* do CDev init for the gyro device node, keep it optional */ + /* XXX test this when another mag is used */ + /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); if (mag_ret != OK) { @@ -762,6 +803,7 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) int LSM303D::set_range(unsigned max_dps) { + /* XXX implement this */ // uint8_t bits = REG4_BDU; // // if (max_dps == 0) @@ -793,8 +835,7 @@ LSM303D::set_range(unsigned max_dps) int LSM303D::set_samplerate(unsigned frequency) { - _current_accel_rate = 100; - + /* XXX implement this */ // uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; // // if (frequency == 0) @@ -869,16 +910,6 @@ void LSM303D::measure() { /* status register and data as read back from the device */ -//#pragma pack(push, 1) -// struct { -// uint8_t cmd; -// uint16_t temp; -// uint8_t status; -// int16_t x; -// int16_t y; -// int16_t z; -// } raw_report_mag; -//#pragma pack(pop) #pragma pack(push, 1) struct { @@ -899,6 +930,7 @@ LSM303D::measure() raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -967,6 +999,7 @@ LSM303D::mag_measure() raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1001,6 +1034,7 @@ LSM303D::mag_measure() if (_next_mag_report == _oldest_mag_report) INCREMENT(_oldest_mag_report, _num_mag_reports); + /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1149,6 +1183,7 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); + /* XXX fix the test output */ // warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); // warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); // warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); @@ -1174,6 +1209,7 @@ test() if (sz != sizeof(m_report)) err(1, "immediate read failed"); + /* XXX fix the test output */ warnx("mag x: \t%d\traw", (int)m_report.x_raw); warnx("mag y: \t%d\traw", (int)m_report.y_raw); warnx("mag z: \t%d\traw", (int)m_report.z_raw); -- cgit v1.2.3 From c25248f1af8d6c4d12b3d7d0f9d42e58e28a6c22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Apr 2013 12:00:51 +0200 Subject: Fixed RGB led warnings and error handling --- src/device/rgbled/rgbled.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp index c3b92ba7e..a0db30f48 100644 --- a/src/device/rgbled/rgbled.cpp +++ b/src/device/rgbled/rgbled.cpp @@ -219,9 +219,13 @@ RGBLED::info() ret = get(on, not_powersave, r, g, b); - /* we don't care about power-save mode */ - log("State: %s", on ? "ON" : "OFF"); - log("Red: %d, Green: %d, Blue: %d", r, g, b); + if (ret == OK) { + /* we don't care about power-save mode */ + log("state: %s", on ? "ON" : "OFF"); + log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + } else { + warnx("failed to read led"); + } return ret; } @@ -394,6 +398,7 @@ RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) if (ret == OK) { on = result[0] & SETTING_ENABLE; not_powersave = result[0] & SETTING_NOT_POWERSAVE; + /* XXX check, looks wrong */ r = (result[0] & 0x0f)*255/15; g = (result[1] & 0xf0)*255/15; b = (result[1] & 0x0f)*255/15; @@ -402,12 +407,14 @@ RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) return ret; } +void rgbled_usage(); + void rgbled_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'test', 'info', 'off'\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --ddr addr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --ddr addr (9)"); } int -- cgit v1.2.3 From e2c30d7c1de93be33d700aa0dc2ffba23df33cdd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Apr 2013 12:12:39 +0200 Subject: Added include dir for RGB led --- src/device/rgbled/rgbled.cpp | 4 +-- src/include/device/rgbled.h | 67 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+), 2 deletions(-) create mode 100644 src/include/device/rgbled.h diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp index a0db30f48..923e0a14f 100644 --- a/src/device/rgbled/rgbled.cpp +++ b/src/device/rgbled/rgbled.cpp @@ -60,11 +60,11 @@ #include #include +#include "device/rgbled.h" + #define LED_ONTIME 120 #define LED_OFFTIME 120 -#define RGBLED_DEVICE_PATH "/dev/rgbled" - #define ADDR 0x55 /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ #define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h new file mode 100644 index 000000000..a18920892 --- /dev/null +++ b/src/include/device/rgbled.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 rgbled.h + * + * RGB led device API + */ + +#pragma once + +#include +#include + +/* more devices will be 1, 2, etc */ +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +/* + * ioctl() definitions + */ + +#define _RGBLEDIOCBASE (0x2900) +#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) + +/** play the named script in *(char *)arg, repeating forever */ +#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) + +/** play the numbered script in (arg), repeating forever */ +#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) + +/** + * Set the user script; (arg) is a pointer to an array of script lines, + * where each line is an array of four bytes giving , , arg[0-2] + * + * The script is terminated by a zero command. + */ +#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) -- cgit v1.2.3 From dc11bcfb425b8bbe2860f585e296f53e08f619ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Apr 2013 12:56:22 +0200 Subject: Disabled full JTAG port to free PA15 for tone alarm --- nuttx/configs/px4fmuv2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 8e9feef65..beeb47551 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -107,9 +107,9 @@ CONFIG_STM32_STM32F427=y # CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled # CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # # On-chip CCM SRAM configuration -- cgit v1.2.3 From aa369dfef167f0fd1250350dbf73c8208691fd67 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 10:20:03 -0700 Subject: Fix command registration for modules. 'rgbled test' works now. --- makefiles/firmware.mk | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index dad57d531..c34382ae0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -298,10 +298,12 @@ endif # BUILTIN_CSRC = $(WORK_DIR)builtin_commands.c -# add command definitions from modules -BUILTIN_COMMAND_FILES := $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*) -BUILTIN_COMMANDS += $(subst COMMAND.,,$(notdir $(BUILTIN_COMMAND_FILES))) +# command definitions from modules (may be empty at Makefile parsing time...) +MODULE_COMMANDS = $(subst COMMAND.,,$(notdir $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*))) +# We must have at least one pre-defined builtin command in order to generate +# any of this. +# ifneq ($(BUILTIN_COMMANDS),) # (BUILTIN_PROTO,,) @@ -315,17 +317,19 @@ define BUILTIN_DEF endef # Don't generate until modules have updated their command files -$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(BUILTIN_COMMAND_FILES) +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) @$(ECHO) %% generating $@ $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include ' >> $@ $(Q) $(ECHO) '#include ' >> $@ $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) $(Q) $(ECHO) 'const struct builtin_s g_builtins[] = {' >> $@ $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) $(Q) $(ECHO) ' {NULL, 0, 0, NULL}' >> $@ $(Q) $(ECHO) '};' >> $@ - $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS));' >> $@ + $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS) $(MODULE_COMMANDS));' >> $@ SRCS += $(BUILTIN_CSRC) -- cgit v1.2.3 From e5fa9dbcea5defb49506dc60a35e134b3736bbb6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 11:16:24 -0700 Subject: Move the LSM303D driver over into the new world. --- apps/drivers/lsm303d/Makefile | 42 -- apps/drivers/lsm303d/lsm303d.cpp | 1290 ---------------------------------- makefiles/config_px4fmuv2_default.mk | 1 + src/device/lsm303d/lsm303d.cpp | 1290 ++++++++++++++++++++++++++++++++++ src/device/lsm303d/module.mk | 6 + 5 files changed, 1297 insertions(+), 1332 deletions(-) delete mode 100644 apps/drivers/lsm303d/Makefile delete mode 100644 apps/drivers/lsm303d/lsm303d.cpp create mode 100644 src/device/lsm303d/lsm303d.cpp create mode 100644 src/device/lsm303d/module.mk diff --git a/apps/drivers/lsm303d/Makefile b/apps/drivers/lsm303d/Makefile deleted file mode 100644 index 542a9bd40..000000000 --- a/apps/drivers/lsm303d/Makefile +++ /dev/null @@ -1,42 +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. -# -############################################################################ - -# -# Makefile to build the LSM303D driver. -# - -APPNAME = lsm303d -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/lsm303d/lsm303d.cpp b/apps/drivers/lsm303d/lsm303d.cpp deleted file mode 100644 index 32030a1f7..000000000 --- a/apps/drivers/lsm303d/lsm303d.cpp +++ /dev/null @@ -1,1290 +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 lsm303d.cpp - * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - - - -/* register addresses: A: accel, M: mag, T: temp */ -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 - -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D - -#define ADDR_OUT_TEMP_A 0x26 -#define ADDR_STATUS_A 0x27 -#define ADDR_OUT_X_L_A 0x28 -#define ADDR_OUT_X_H_A 0x29 -#define ADDR_OUT_Y_L_A 0x2A -#define ADDR_OUT_Y_H_A 0x2B -#define ADDR_OUT_Z_L_A 0x2C -#define ADDR_OUT_Z_H_A 0x2D - -#define ADDR_CTRL_REG0 0x1F -#define ADDR_CTRL_REG1 0x20 -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define ADDR_CTRL_REG5 0x24 -#define ADDR_CTRL_REG7 0x26 - -#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) - -#define REG1_CONT_UPDATE_A (0<<3) -#define REG1_Z_ENABLE_A (1<<2) -#define REG1_Y_ENABLE_A (1<<1) -#define REG1_X_ENABLE_A (1<<0) - -#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) -#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) -#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) -#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) - -#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) -#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) -#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) -#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) -#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) - -#define REG5_ENABLE_T (1<<7) - -#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) -#define REG5_RES_LOW_M ((0<<6) | (0<<5)) - -#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) -#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) -#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) -#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) -#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) -#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) -#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) - -#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) -#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) -#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) -#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) - -#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) - - -#define INT_CTRL_M 0x12 -#define INT_SRC_M 0x13 - - -extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } - - -class LSM303D_mag; - -class LSM303D : public device::SPI -{ -public: - LSM303D(int bus, const char* path, spi_dev_e device); - virtual ~LSM303D(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - - friend class LSM303D_mag; - - virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); - -private: - - LSM303D_mag *_mag; - - struct hrt_call _accel_call; - struct hrt_call _mag_call; - - unsigned _call_accel_interval; - unsigned _call_mag_interval; - - unsigned _num_accel_reports; - volatile unsigned _next_accel_report; - volatile unsigned _oldest_accel_report; - struct accel_report *_accel_reports; - - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _num_mag_reports; - volatile unsigned _next_mag_report; - volatile unsigned _oldest_mag_report; - struct mag_report *_mag_reports; - - struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; - orb_advert_t _mag_topic; - - unsigned _current_accel_rate; - unsigned _current_accel_range; - - unsigned _current_mag_rate; - unsigned _current_mag_range; - - perf_counter_t _accel_sample_perf; - perf_counter_t _mag_sample_perf; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); - - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - void measure(); - - /** - * Fetch mag measurements from the sensor and update the report ring. - */ - void mag_measure(); - - /** - * Read a register from the LSM303D - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the LSM303D - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the LSM303D - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Set the LSM303D measurement range. - * - * @param max_dps The measurement range is set to permit reading at least - * this rate in degrees per second. - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_range(unsigned max_dps); - - /** - * Set the LSM303D internal sampling frequency. - * - * @param frequency The internal sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int set_samplerate(unsigned frequency); -}; - -/** - * Helper class implementing the mag driver node. - */ -class LSM303D_mag : public device::CDev -{ -public: - LSM303D_mag(LSM303D *parent); - ~LSM303D_mag(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - -protected: - friend class LSM303D; - - void parent_poll_notify(); -private: - LSM303D *_parent; - - void measure(); - - void measure_trampoline(void *arg); -}; - - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - - -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), - _mag(new LSM303D_mag(this)), - _call_accel_interval(0), - _call_mag_interval(0), - _num_accel_reports(0), - _next_accel_report(0), - _oldest_accel_report(0), - _accel_reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _num_mag_reports(0), - _next_mag_report(0), - _oldest_mag_report(0), - _mag_reports(nullptr), - _mag_range_scale(0.0f), - _mag_range_ga(0.0f), - _current_accel_rate(0), - _current_accel_range(0), - _current_mag_rate(0), - _current_mag_range(0), - _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) -{ - // enable debug() calls - _debug_enabled = true; - - /* XXX fix this default values */ - _accel_range_scale = 1.0f; - _mag_range_scale = 1.0f; - - // default scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - _mag_scale.x_offset = 0; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; - _mag_scale.z_scale = 1.0f; -} - -LSM303D::~LSM303D() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_accel_reports != nullptr) - delete[] _accel_reports; - if (_mag_reports != nullptr) - delete[] _mag_reports; - - delete _mag; - - /* delete the perf counter */ - perf_free(_accel_sample_perf); - perf_free(_mag_sample_perf); -} - -int -LSM303D::init() -{ - int ret = ERROR; - int mag_ret; - int fd_mag; - - /* do SPI init (and probe) first */ - if (SPI::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_accel_reports = 2; - _oldest_accel_report = _next_accel_report = 0; - _accel_reports = new struct accel_report[_num_accel_reports]; - - if (_accel_reports == nullptr) - goto out; - - /* advertise accel topic */ - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); - - _num_mag_reports = 2; - _oldest_mag_report = _next_mag_report = 0; - _mag_reports = new struct mag_report[_num_mag_reports]; - - if (_mag_reports == nullptr) - goto out; - - /* advertise mag topic */ - memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - - /* XXX do this with ioctls */ - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); - - /* XXX should we enable FIFO */ - - set_range(500); /* default to 500dps */ - set_samplerate(0); /* max sample rate */ - -// _current_accel_rate = 100; - - /* XXX test this when another mag is used */ - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; - } - - ret = OK; -out: - return ret; -} - -int -LSM303D::probe() -{ - /* read dummy value to void to clear SPI statemachine on sensor */ - (void)read_reg(ADDR_WHO_AM_I); - - /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) - return OK; - - return -EIO; -} - -ssize_t -LSM303D::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct accel_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_call_accel_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_accel_report != _next_accel_report) { - memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); - ret += sizeof(_accel_reports[0]); - INCREMENT(_oldest_accel_report, _num_accel_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _oldest_accel_report = _next_accel_report = 0; - measure(); - - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); - ret = sizeof(*_accel_reports); - - return ret; -} - -ssize_t -LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct mag_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_call_mag_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_mag_report != _next_mag_report) { - memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); - ret += sizeof(_mag_reports[0]); - INCREMENT(_oldest_mag_report, _num_mag_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _oldest_mag_report = _next_mag_report = 0; - measure(); - - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); - ret = sizeof(*_mag_reports); - - return ret; -} - -int -LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_accel_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_accel_interval; - - case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; - - case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -int -LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_mag_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* 50 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_mag_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = ticks; - - - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_mag_interval; - case SENSORIOCSQUEUEDEPTH: - case SENSORIOCGQUEUEDEPTH: - case SENSORIOCRESET: - return ioctl(filp, cmd, arg); - - case MAGIOCSSAMPLERATE: -// case MAGIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; - - case MAGIOCSLOWPASS: -// case MAGIOCGLOWPASS: - /* XXX not implemented */ -// _set_dlpf_filter((uint16_t)arg); - return -EINVAL; - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - case MAGIOCSRANGE: -// case MAGIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _mag_range_scale = xx - // _mag_range_ga = xx - return -EINVAL; - - case MAGIOCSELFTEST: - /* XXX not implemented */ -// return self_test(); - return -EINVAL; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -uint8_t -LSM303D::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -LSM303D::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int -LSM303D::set_range(unsigned max_dps) -{ - /* XXX implement this */ -// uint8_t bits = REG4_BDU; -// -// if (max_dps == 0) -// max_dps = 2000; -// -// if (max_dps <= 250) { -// _current_range = 250; -// bits |= RANGE_250DPS; -// -// } else if (max_dps <= 500) { -// _current_range = 500; -// bits |= RANGE_500DPS; -// -// } else if (max_dps <= 2000) { -// _current_range = 2000; -// bits |= RANGE_2000DPS; -// -// } else { -// return -EINVAL; -// } -// -// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; -// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; -// write_reg(ADDR_CTRL_REG4, bits); - - return OK; -} - -int -LSM303D::set_samplerate(unsigned frequency) -{ - /* XXX implement this */ -// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; -// -// if (frequency == 0) -// frequency = 760; -// -// if (frequency <= 95) { -// _current_rate = 95; -// bits |= RATE_95HZ_LP_25HZ; -// -// } else if (frequency <= 190) { -// _current_rate = 190; -// bits |= RATE_190HZ_LP_25HZ; -// -// } else if (frequency <= 380) { -// _current_rate = 380; -// bits |= RATE_380HZ_LP_30HZ; -// -// } else if (frequency <= 760) { -// _current_rate = 760; -// bits |= RATE_760HZ_LP_30HZ; -// -// } else { -// return -EINVAL; -// } -// -// write_reg(ADDR_CTRL_REG1, bits); - - return OK; -} - -void -LSM303D::start() -{ - /* make sure we are stopped first */ - stop(); - - /* reset the report ring */ - _oldest_accel_report = _next_accel_report = 0; - _oldest_mag_report = _next_mag_report = 0; - - /* start polling at the specified rate */ - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); -} - -void -LSM303D::stop() -{ - hrt_cancel(&_accel_call); - hrt_cancel(&_mag_call); -} - -void -LSM303D::measure_trampoline(void *arg) -{ - LSM303D *dev = (LSM303D *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -LSM303D::mag_measure_trampoline(void *arg) -{ - LSM303D *dev = (LSM303D *)arg; - - /* make another measurement */ - dev->mag_measure(); -} - -void -LSM303D::measure() -{ - /* status register and data as read back from the device */ - -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_accel_report; -#pragma pack(pop) - - accel_report *accel_report = &_accel_reports[_next_accel_report]; - - /* start the performance counter */ - perf_begin(_accel_sample_perf); - - /* fetch data from the sensor */ - raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); - - /* XXX adapt the comment to specs */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - - accel_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - accel_report->x_raw = raw_accel_report.x; - accel_report->y_raw = raw_accel_report.y; - accel_report->z_raw = raw_accel_report.z; - - accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_accel_report == _oldest_accel_report) - INCREMENT(_oldest_accel_report, _num_accel_reports); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); - - /* stop the perf counter */ - perf_end(_accel_sample_perf); -} - -void -LSM303D::mag_measure() -{ - /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_mag_report; -#pragma pack(pop) - - mag_report *mag_report = &_mag_reports[_next_mag_report]; - - /* start the performance counter */ - perf_begin(_mag_sample_perf); - - /* fetch data from the sensor */ - raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); - - /* XXX adapt the comment to specs */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - - mag_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - mag_report->x_raw = raw_mag_report.x; - mag_report->y_raw = raw_mag_report.y; - mag_report->z_raw = raw_mag_report.z; - mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_mag_report, _num_mag_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_mag_report == _oldest_mag_report) - INCREMENT(_oldest_mag_report, _num_mag_reports); - - /* XXX please check this poll_notify, is it the right one? */ - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); - - /* stop the perf counter */ - perf_end(_mag_sample_perf); -} - -void -LSM303D::print_info() -{ - perf_print_counter(_accel_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); - perf_print_counter(_mag_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); -} - -LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), - _parent(parent) -{ -} - -LSM303D_mag::~LSM303D_mag() -{ -} - -void -LSM303D_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -ssize_t -LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->mag_read(filp, buffer, buflen); -} - -int -LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - return _parent->mag_ioctl(filp, cmd, arg); -} - -void -LSM303D_mag::measure() -{ - _parent->mag_measure(); -} - -void -LSM303D_mag::measure_trampoline(void *arg) -{ - _parent->mag_measure_trampoline(arg); -} - -/** - * Local functions in support of the shell command. - */ -namespace lsm303d -{ - -LSM303D *g_dev; - -void start(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd, fd_mag; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); - - /* don't fail if open cannot be opened */ - if (0 <= fd_mag) { - if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - } - - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int fd_accel = -1; - struct accel_report a_report; - ssize_t sz; - - /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd_accel, &a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) - err(1, "immediate read failed"); - - /* XXX fix the test output */ -// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); -// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); -// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); - warnx("accel x: \t%d\traw", (int)a_report.x_raw); - warnx("accel y: \t%d\traw", (int)a_report.y_raw); - warnx("accel z: \t%d\traw", (int)a_report.z_raw); -// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); - - - - int fd_mag = -1; - struct mag_report m_report; - - /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd_mag, &m_report, sizeof(m_report)); - - if (sz != sizeof(m_report)) - err(1, "immediate read failed"); - - /* XXX fix the test output */ - warnx("mag x: \t%d\traw", (int)m_report.x_raw); - warnx("mag y: \t%d\traw", (int)m_report.y_raw); - warnx("mag z: \t%d\traw", (int)m_report.z_raw); - - /* XXX add poll-rate tests here too */ - -// reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running\n"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - - -} // namespace - -int -lsm303d_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - - */ - if (!strcmp(argv[1], "start")) - lsm303d::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - lsm303d::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - lsm303d::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - lsm303d::info(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 0ea0a9047..374c0cfe9 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -11,6 +11,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) # Board support modules # MODULES += device/rgbled +MODULES += device/lsm303d # # Transitional support - add commands from the NuttX export archive. diff --git a/src/device/lsm303d/lsm303d.cpp b/src/device/lsm303d/lsm303d.cpp new file mode 100644 index 000000000..32030a1f7 --- /dev/null +++ b/src/device/lsm303d/lsm303d.cpp @@ -0,0 +1,1290 @@ +/**************************************************************************** + * + * 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 lsm303d.cpp + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + + + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_L_T 0x05 +#define ADDR_OUT_H_T 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_OUT_TEMP_A 0x26 +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG0 0x1F +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG7 0x26 + +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) + +#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) + +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) + +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) + +#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) +#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) +#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) +#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) + +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) + + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + + +class LSM303D_mag; + +class LSM303D : public device::SPI +{ +public: + LSM303D(int bus, const char* path, spi_dev_e device); + virtual ~LSM303D(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + + friend class LSM303D_mag; + + virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + LSM303D_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_accel_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + + unsigned _num_mag_reports; + volatile unsigned _next_mag_report; + volatile unsigned _oldest_mag_report; + struct mag_report *_mag_reports; + + struct mag_scale _mag_scale; + float _mag_range_scale; + float _mag_range_ga; + orb_advert_t _mag_topic; + + unsigned _current_accel_rate; + unsigned _current_accel_range; + + unsigned _current_mag_rate; + unsigned _current_mag_range; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ + static void mag_measure_trampoline(void *arg); + + /** + * Fetch accel measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Fetch mag measurements from the sensor and update the report ring. + */ + void mag_measure(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D measurement range. + * + * @param max_dps The measurement range is set to permit reading at least + * this rate in degrees per second. + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_dps); + + /** + * Set the LSM303D internal sampling frequency. + * + * @param frequency The internal sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); +}; + +/** + * Helper class implementing the mag driver node. + */ +class LSM303D_mag : public device::CDev +{ +public: + LSM303D_mag(LSM303D *parent); + ~LSM303D_mag(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class LSM303D; + + void parent_poll_notify(); +private: + LSM303D *_parent; + + void measure(); + + void measure_trampoline(void *arg); +}; + + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + _mag(new LSM303D_mag(this)), + _call_accel_interval(0), + _call_mag_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), + _num_mag_reports(0), + _next_mag_report(0), + _oldest_mag_report(0), + _mag_reports(nullptr), + _mag_range_scale(0.0f), + _mag_range_ga(0.0f), + _current_accel_rate(0), + _current_accel_range(0), + _current_mag_rate(0), + _current_mag_range(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) +{ + // enable debug() calls + _debug_enabled = true; + + /* XXX fix this default values */ + _accel_range_scale = 1.0f; + _mag_range_scale = 1.0f; + + // default scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0; + _mag_scale.z_scale = 1.0f; +} + +LSM303D::~LSM303D() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete[] _accel_reports; + if (_mag_reports != nullptr) + delete[] _mag_reports; + + delete _mag; + + /* delete the perf counter */ + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); +} + +int +LSM303D::init() +{ + int ret = ERROR; + int mag_ret; + int fd_mag; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; + + if (_accel_reports == nullptr) + goto out; + + /* advertise accel topic */ + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + + _num_mag_reports = 2; + _oldest_mag_report = _next_mag_report = 0; + _mag_reports = new struct mag_report[_num_mag_reports]; + + if (_mag_reports == nullptr) + goto out; + + /* advertise mag topic */ + memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + + /* XXX do this with ioctls */ + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); + + /* XXX should we enable FIFO */ + + set_range(500); /* default to 500dps */ + set_samplerate(0); /* max sample rate */ + +// _current_accel_rate = 100; + + /* XXX test this when another mag is used */ + /* do CDev init for the mag device node, keep it optional */ + mag_ret = _mag->init(); + + if (mag_ret != OK) { + _mag_topic = -1; + } + + ret = OK; +out: + return ret; +} + +int +LSM303D::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +LSM303D::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_accel_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_accel_report = _next_accel_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); + + return ret; +} + +ssize_t +LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_mag_report != _next_mag_report) { + memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); + ret += sizeof(_mag_reports[0]); + INCREMENT(_oldest_mag_report, _num_mag_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_mag_report = _next_mag_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); + ret = sizeof(*_mag_reports); + + return ret; +} + +int +LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_accel_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_accel_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_accel_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_accel_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 50 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + case SENSORIOCSQUEUEDEPTH: + case SENSORIOCGQUEUEDEPTH: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case MAGIOCSSAMPLERATE: +// case MAGIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case MAGIOCSLOWPASS: +// case MAGIOCGLOWPASS: + /* XXX not implemented */ +// _set_dlpf_filter((uint16_t)arg); + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: +// case MAGIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _mag_range_scale = xx + // _mag_range_ga = xx + return -EINVAL; + + case MAGIOCSELFTEST: + /* XXX not implemented */ +// return self_test(); + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +LSM303D::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +LSM303D::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +LSM303D::set_range(unsigned max_dps) +{ + /* XXX implement this */ +// uint8_t bits = REG4_BDU; +// +// if (max_dps == 0) +// max_dps = 2000; +// +// if (max_dps <= 250) { +// _current_range = 250; +// bits |= RANGE_250DPS; +// +// } else if (max_dps <= 500) { +// _current_range = 500; +// bits |= RANGE_500DPS; +// +// } else if (max_dps <= 2000) { +// _current_range = 2000; +// bits |= RANGE_2000DPS; +// +// } else { +// return -EINVAL; +// } +// +// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; +// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; +// write_reg(ADDR_CTRL_REG4, bits); + + return OK; +} + +int +LSM303D::set_samplerate(unsigned frequency) +{ + /* XXX implement this */ +// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; +// +// if (frequency == 0) +// frequency = 760; +// +// if (frequency <= 95) { +// _current_rate = 95; +// bits |= RATE_95HZ_LP_25HZ; +// +// } else if (frequency <= 190) { +// _current_rate = 190; +// bits |= RATE_190HZ_LP_25HZ; +// +// } else if (frequency <= 380) { +// _current_rate = 380; +// bits |= RATE_380HZ_LP_30HZ; +// +// } else if (frequency <= 760) { +// _current_rate = 760; +// bits |= RATE_760HZ_LP_30HZ; +// +// } else { +// return -EINVAL; +// } +// +// write_reg(ADDR_CTRL_REG1, bits); + + return OK; +} + +void +LSM303D::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_accel_report = _next_accel_report = 0; + _oldest_mag_report = _next_mag_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); +} + +void +LSM303D::stop() +{ + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); +} + +void +LSM303D::measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +LSM303D::mag_measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + +void +LSM303D::measure() +{ + /* status register and data as read back from the device */ + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; +#pragma pack(pop) + + accel_report *accel_report = &_accel_reports[_next_accel_report]; + + /* start the performance counter */ + perf_begin(_accel_sample_perf); + + /* fetch data from the sensor */ + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + /* XXX adapt the comment to specs */ + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + accel_report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + accel_report->x_raw = raw_accel_report.x; + accel_report->y_raw = raw_accel_report.y; + accel_report->z_raw = raw_accel_report.z; + + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_accel_report, _num_accel_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +LSM303D::mag_measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report *mag_report = &_mag_reports[_next_mag_report]; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* XXX adapt the comment to specs */ + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + mag_report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + mag_report->x_raw = raw_mag_report.x; + mag_report->y_raw = raw_mag_report.y; + mag_report->z_raw = raw_mag_report.z; + mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_mag_report, _num_mag_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_mag_report == _oldest_mag_report) + INCREMENT(_oldest_mag_report, _num_mag_reports); + + /* XXX please check this poll_notify, is it the right one? */ + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + + /* stop the perf counter */ + perf_end(_mag_sample_perf); +} + +void +LSM303D::print_info() +{ + perf_print_counter(_accel_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); + perf_print_counter(_mag_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); +} + +LSM303D_mag::LSM303D_mag(LSM303D *parent) : + CDev("LSM303D_mag", MAG_DEVICE_PATH), + _parent(parent) +{ +} + +LSM303D_mag::~LSM303D_mag() +{ +} + +void +LSM303D_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->mag_ioctl(filp, cmd, arg); +} + +void +LSM303D_mag::measure() +{ + _parent->mag_measure(); +} + +void +LSM303D_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd, fd_mag; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + } + + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_accel = -1; + struct accel_report a_report; + ssize_t sz; + + /* get the driver */ + fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd_accel < 0) + err(1, "%s open failed", ACCEL_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_accel, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) + err(1, "immediate read failed"); + + /* XXX fix the test output */ +// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); +// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); +// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); + warnx("accel x: \t%d\traw", (int)a_report.x_raw); + warnx("accel y: \t%d\traw", (int)a_report.y_raw); + warnx("accel z: \t%d\traw", (int)a_report.z_raw); +// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + + + + int fd_mag = -1; + struct mag_report m_report; + + /* get the driver */ + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) + err(1, "%s open failed", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_mag, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) + err(1, "immediate read failed"); + + /* XXX fix the test output */ + warnx("mag x: \t%d\traw", (int)m_report.x_raw); + warnx("mag y: \t%d\traw", (int)m_report.y_raw); + warnx("mag z: \t%d\traw", (int)m_report.z_raw); + + /* XXX add poll-rate tests here too */ + +// reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + lsm303d::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + lsm303d::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + lsm303d::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + lsm303d::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/device/lsm303d/module.mk b/src/device/lsm303d/module.mk new file mode 100644 index 000000000..e93b1e331 --- /dev/null +++ b/src/device/lsm303d/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = lsm303d +SRCS = lsm303d.cpp -- cgit v1.2.3 From aa09ebd7d3cb0d06781470e400e61c75b449985b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 11:16:54 -0700 Subject: Share the ROMFS prototype betwen FMUv1 and v2 --- ROMFS/px4fmu_common/logging/logconv.m | 224 +++++++++++++++++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 64 +++++++++ ROMFS/px4fmu_common/mixers/FMU_AET.mix | 60 ++++++++ ROMFS/px4fmu_common/mixers/FMU_Q.mix | 52 +++++++ ROMFS/px4fmu_common/mixers/FMU_RET.mix | 53 +++++++ ROMFS/px4fmu_common/mixers/FMU_X5.mix | 50 +++++++ ROMFS/px4fmu_common/mixers/FMU_delta.mix | 50 +++++++ ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_octo_+.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_octo_x.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_pass.mix | 23 +++ ROMFS/px4fmu_common/mixers/FMU_quad_+.mix | 7 + ROMFS/px4fmu_common/mixers/FMU_quad_x.mix | 7 + ROMFS/px4fmu_common/mixers/README | 154 ++++++++++++++++++++ ROMFS/px4fmu_default/logging/logconv.m | 224 ----------------------------- ROMFS/px4fmu_default/mixers/FMU_AERT.mix | 64 --------- ROMFS/px4fmu_default/mixers/FMU_AET.mix | 60 -------- ROMFS/px4fmu_default/mixers/FMU_Q.mix | 52 ------- ROMFS/px4fmu_default/mixers/FMU_RET.mix | 53 ------- ROMFS/px4fmu_default/mixers/FMU_X5.mix | 50 ------- ROMFS/px4fmu_default/mixers/FMU_delta.mix | 50 ------- ROMFS/px4fmu_default/mixers/FMU_hex_+.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_hex_x.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_octo_+.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_octo_x.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_pass.mix | 23 --- ROMFS/px4fmu_default/mixers/FMU_quad_+.mix | 7 - ROMFS/px4fmu_default/mixers/FMU_quad_x.mix | 7 - ROMFS/px4fmu_default/mixers/README | 154 -------------------- makefiles/config_px4fmu_default.mk | 2 +- makefiles/config_px4fmuv2_default.mk | 2 +- makefiles/firmware.mk | 7 +- makefiles/module.mk | 1 - 34 files changed, 779 insertions(+), 777 deletions(-) create mode 100755 ROMFS/px4fmu_common/logging/logconv.m create mode 100644 ROMFS/px4fmu_common/mixers/FMU_AERT.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_AET.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_Q.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_RET.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_X5.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_delta.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_hex_+.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_hex_x.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_octo_+.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_octo_x.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_pass.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quad_+.mix create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quad_x.mix create mode 100644 ROMFS/px4fmu_common/mixers/README delete mode 100755 ROMFS/px4fmu_default/logging/logconv.m delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_AERT.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_AET.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_Q.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_RET.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_X5.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_delta.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_hex_+.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_hex_x.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_octo_+.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_octo_x.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_pass.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_quad_+.mix delete mode 100644 ROMFS/px4fmu_default/mixers/FMU_quad_x.mix delete mode 100644 ROMFS/px4fmu_default/mixers/README diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m new file mode 100755 index 000000000..92ee01413 --- /dev/null +++ b/ROMFS/px4fmu_common/logging/logconv.m @@ -0,0 +1,224 @@ +% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +% Clear everything +clc +clear all +close all + +% Set the path to your sysvector.bin file here +filePath = 'sysvector.bin'; + +% Work around a Matlab bug (not related to PX4) +% where timestamps from 1.1.1970 do not allow to +% read the file's size +if ismac + system('touch -t 201212121212.12 sysvector.bin'); +end + +%%%%%%%%%%%%%%%%%%%%%%% +% SYSTEM VECTOR +% +% //All measurements in NED frame +% +% uint64_t timestamp; //[us] +% float gyro[3]; //[rad/s] +% float accel[3]; //[m/s^2] +% float mag[3]; //[gauss] +% float baro; //pressure [millibar] +% float baro_alt; //altitude above MSL [meter] +% float baro_temp; //[degree celcius] +% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] +% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) +% float vbat; //battery voltage in [volt] +% float bat_current - current drawn from battery at this time instant +% float bat_discharged - discharged energy in mAh +% float adc[4]; //ADC ports [volt] +% float local_position[3]; //tangent plane mapping into x,y,z [m] +% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] +% float attitude[3]; //pitch, roll, yaw [rad] +% float rotMatrix[9]; //unitvectors +% float actuator_control[4]; //unitvector +% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] +% float diff_pressure; - pressure difference in millibar +% float ind_airspeed; +% float true_airspeed; + +% Definition of the logged values +logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); +logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); +logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); + +% First get length of one line +columns = length(logFormat); +lineLength = 0; + +for i=1:columns + lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; +end + + +if exist(filePath, 'file') + + fileInfo = dir(filePath); + fileSize = fileInfo.bytes; + + elements = int64(fileSize./(lineLength)); + + fid = fopen(filePath, 'r'); + offset = 0; + for i=1:columns + % using fread with a skip speeds up the import drastically, do not + % import the values one after the other + sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... + fid, ... + [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... + lineLength - logFormat{i}.bytes*logFormat{i}.array, ... + logFormat{i}.machineformat) ... + ); + offset = offset + logFormat{i}.bytes*logFormat{i}.array; + fseek(fid, offset,'bof'); + end + + % shot the flight time + time_us = sysvector.timestamp(end) - sysvector.timestamp(1); + time_s = time_us*1e-6; + time_m = time_s/60; + + % close the logfile + fclose(fid); + + disp(['end log2matlab conversion' char(10)]); +else + disp(['file: ' filePath ' does not exist' char(10)]); +end + +%% Plot GPS RAW measurements + +% Only plot GPS data if available +if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0 + figure('units','normalized','outerposition',[0 0 1 1]) + plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3)); +end + + +%% Plot optical flow trajectory + +flow_sz = size(sysvector.timestamp); +flow_elements = flow_sz(1); + +xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms] + + +%calc dt +dt = zeros(flow_elements,1); +for i = 1:flow_elements-1 + dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s] +end +dt(1,1) = mean(dt); + + +global_speed = zeros(flow_elements,3); + +%calc global speed (with rot matrix) +for i = 1:flow_elements + rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]'; + speedX = sysvector.optical_flow(i,3); + speedY = sysvector.optical_flow(i,4); + + relSpeed = [-speedY,speedX,0]; + global_speed(i,:) = relSpeed * rotM; +end + + + +px = zeros(flow_elements,1); +py = zeros(flow_elements,1); +distance = 0; + +last_vx = 0; +last_vy = 0; +elem_cnt = 0; + +% Very basic accumulation, stops on bad flow quality +for i = 1:flow_elements + if sysvector.optical_flow(i,6) > 5 + px(i,1) = global_speed(i,1)*dt(i,1); + py(i,1) = global_speed(i,2)*dt(i,1); + distance = distance + norm([px(i,1) py(i,1)]); + last_vx = px(i,1); + last_vy = py(i,1); + else + px(i,1) = last_vx; + py(i,1) = last_vy; + last_vx = last_vx*0.95; + last_vy = last_vy*0.95; + end +end + +px_sum = cumsum(px); +py_sum = cumsum(py); +time = cumsum(dt); + +figure() +set(gca, 'Units','normal'); + +plot(py_sum, px_sum, '-blue', 'LineWidth',2); +axis equal; +% set title and axis captions +xlabel('X position (meters)','fontsize',14) +ylabel('Y position (meters)','fontsize',14) +% mark begin and end +hold on +plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,... +'MarkerEdgeColor','k',... +'MarkerFaceColor','g',... +'MarkerSize',10) +hold on +plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,... +'MarkerEdgeColor','k',... +'MarkerFaceColor','b',... +'MarkerSize',10) +% add total length as annotation +set(gca,'fontsize',13); +legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60)); +title('Optical Flow Position Integration', 'fontsize', 15); + +figure() +plot(time, sysvector.optical_flow(:,5), 'blue'); +axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]); +xlabel('seconds','fontsize',14); +ylabel('m','fontsize',14); +set(gca,'fontsize',13); +title('Ultrasound Altitude', 'fontsize', 15); + + +figure() +plot(time, global_speed(:,2), 'red'); +hold on; +plot(time, global_speed(:,1), 'blue'); +legend('y velocity (m/s)', 'x velocity (m/s)'); +xlabel('seconds','fontsize',14); +ylabel('m/s','fontsize',14); +set(gca,'fontsize',13); +title('Optical Flow Velocity', 'fontsize', 15); diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix new file mode 100644 index 000000000..75e82bb00 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -0,0 +1,64 @@ +Aileron/rudder/elevator/throttle mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the rudder to output 2 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix new file mode 100644 index 000000000..20cb88b91 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix @@ -0,0 +1,60 @@ +Aileron/elevator/throttle mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, elevator and throttle controls using PX4FMU. The configuration assumes +the aileron servo(s) are connected to PX4FMU servo output 0, the elevator to +output 1 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +Alternatively, output 2 could be used as a second aileron servo output with +separate mixing. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix new file mode 100644 index 000000000..ebcb66b24 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -0,0 +1,52 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Bormatec Camflyer Q + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -5000 -8000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -8000 -5000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix new file mode 100644 index 000000000..95beb8927 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix @@ -0,0 +1,53 @@ +Rudder/elevator/throttle mixer for PX4FMU +========================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +rudder, elevator and throttle controls using PX4FMU. The configuration assumes +the rudder servo is connected to PX4FMU servo output 0, the elevator to output 1 +and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Rudder mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix new file mode 100644 index 000000000..9f81e1dc3 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -0,0 +1,50 @@ +Delta-wing mixer for PX4FMU +=========================== + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -3000 -5000 0 -10000 10000 +S: 0 1 -5000 -5000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -5000 -3000 0 -10000 10000 +S: 0 1 5000 5000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix new file mode 100644 index 000000000..981466704 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix @@ -0,0 +1,50 @@ +Delta-wing mixer for PX4FMU +=========================== + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 3000 5000 0 -10000 10000 +S: 0 1 5000 5000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 5000 3000 0 -10000 10000 +S: 0 1 -5000 -5000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix new file mode 100644 index 000000000..b5e38ce9e --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the + configuration. All controls +are mixed 100%. + +R: 6+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix new file mode 100644 index 000000000..8e8d122ad --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a hexacopter in the X configuration. All controls +are mixed 100%. + +R: 6x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix new file mode 100644 index 000000000..2cb70e814 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the + configuration. All controls +are mixed 100%. + +R: 8+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix new file mode 100644 index 000000000..edc71f013 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a octocopter in the X configuration. All controls +are mixed 100%. + +R: 8x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_pass.mix b/ROMFS/px4fmu_common/mixers/FMU_pass.mix new file mode 100644 index 000000000..e9a81f2bb --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_pass.mix @@ -0,0 +1,23 @@ +Passthrough mixer for PX4FMU +============================ + +This file defines passthrough mixers suitable for testing. + +Channel group 0, channels 0-3 are passed directly through to the outputs. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix new file mode 100644 index 000000000..dfdf1d58e --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the + configuration. All controls +are mixed 100%. + +R: 4+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix new file mode 100644 index 000000000..12a3bee20 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the X configuration. All controls +are mixed 100%. + +R: 4x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README new file mode 100644 index 000000000..6649c53c2 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/README @@ -0,0 +1,154 @@ +PX4 mixer definitions +===================== + +Files in this directory implement example mixers that can be used as a basis +for customisation, or for general testing purposes. + +Mixer basics +------------ + +Mixers combine control values from various sources (control tasks, user inputs, +etc.) and produce output values suitable for controlling actuators; servos, +motors, switches and so on. + +An actuator derives its value from the combination of one or more control +values. Each of the control values is scaled according to the actuator's +configuration and then combined to produce the actuator value, which may then be +further scaled to suit the specific output type. + +Internally, all scaling is performed using floating point values. Inputs and +outputs are clamped to the range -1.0 to 1.0. + +control control control + | | | + v v v + scale scale scale + | | | + | v | + +-------> mix <------+ + | + scale + | + v + out + +Scaling +------- + +Basic scalers provide linear scaling of the input to the output. + +Each scaler allows the input value to be scaled independently for inputs +greater/less than zero. An offset can be applied to the output, and lower and +upper boundary constraints can be applied. Negative scaling factors cause the +output to be inverted (negative input produces positive output). + +Scaler pseudocode: + +if (input < 0) + output = (input * NEGATIVE_SCALE) + OFFSET +else + output = (input * POSITIVE_SCALE) + OFFSET + +if (output < LOWER_LIMIT) + output = LOWER_LIMIT +if (output > UPPER_LIMIT) + output = UPPER_LIMIT + +Syntax +------ + +Mixer definitions are text files; lines beginning with a single capital letter +followed by a colon are significant. All other lines are ignored, meaning that +explanatory text can be freely mixed with the definitions. + +Each file may define more than one mixer; the allocation of mixers to actuators +is specific to the device reading the mixer definition, and the number of +actuator outputs generated by a mixer is specific to the mixer. + +A mixer begins with a line of the form + + : + +The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a +multirotor mixer, etc. + +Null Mixer +.......... + +A null mixer consumes no controls and generates a single actuator output whose +value is always zero. Typically a null mixer is used as a placeholder in a +collection of mixers in order to achieve a specific pattern of actuator outputs. + +The null mixer definition has the form: + + Z: + +Simple Mixer +............ + +A simple mixer combines zero or more control inputs into a single actuator +output. Inputs are scaled, and the mixing function sums the result before +applying an output scaler. + +A simple mixer definition begins with: + + M: + O: <-ve scale> <+ve scale> + +If is zero, the sum is effectively zero and the mixer will +output a fixed value that is constrained by and . + +The second line defines the output scaler with scaler parameters as discussed +above. Whilst the calculations are performed as floating-point operations, the +values stored in the definition file are scaled by a factor of 10000; i.e. an +offset of -0.5 is encoded as -5000. + +The definition continues with entries describing the control +inputs and their scaling, in the form: + + S: <-ve scale> <+ve scale> + +The value identifies the control group from which the scaler will read, +and the value an offset within that group. These values are specific to +the device reading the mixer definition. + +When used to mix vehicle controls, mixer group zero is the vehicle attitude +control group, and index values zero through three are normally roll, pitch, +yaw and thrust respectively. + +The remaining fields on the line configure the control scaler with parameters as +discussed above. Whilst the calculations are performed as floating-point +operations, the values stored in the definition file are scaled by a factor of +10000; i.e. an offset of -0.5 is encoded as -5000. + +Multirotor Mixer +................ + +The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) +into a set of actuator outputs intended to drive motor speed controllers. + +The mixer definition is a single line of the form: + +R: + +The supported geometries include: + + 4x - quadrotor in X configuration + 4+ - quadrotor in + configuration + 6x - hexcopter in X configuration + 6+ - hexcopter in + configuration + 8x - octocopter in X configuration + 8+ - octocopter in + configuration + +Each of the roll, pitch and yaw scale values determine scaling of the roll, +pitch and yaw controls relative to the thrust control. Whilst the calculations +are performed as floating-point operations, the values stored in the definition +file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. + +Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the +thrust input ranges from 0.0 to 1.0. Output for each actuator is in the +range -1.0 to 1.0. + +In the case where an actuator saturates, all actuator values are rescaled so that +the saturating actuator is limited to 1.0. diff --git a/ROMFS/px4fmu_default/logging/logconv.m b/ROMFS/px4fmu_default/logging/logconv.m deleted file mode 100755 index 92ee01413..000000000 --- a/ROMFS/px4fmu_default/logging/logconv.m +++ /dev/null @@ -1,224 +0,0 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -% Clear everything -clc -clear all -close all - -% Set the path to your sysvector.bin file here -filePath = 'sysvector.bin'; - -% Work around a Matlab bug (not related to PX4) -% where timestamps from 1.1.1970 do not allow to -% read the file's size -if ismac - system('touch -t 201212121212.12 sysvector.bin'); -end - -%%%%%%%%%%%%%%%%%%%%%%% -% SYSTEM VECTOR -% -% //All measurements in NED frame -% -% uint64_t timestamp; //[us] -% float gyro[3]; //[rad/s] -% float accel[3]; //[m/s^2] -% float mag[3]; //[gauss] -% float baro; //pressure [millibar] -% float baro_alt; //altitude above MSL [meter] -% float baro_temp; //[degree celcius] -% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] -% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) -% float vbat; //battery voltage in [volt] -% float bat_current - current drawn from battery at this time instant -% float bat_discharged - discharged energy in mAh -% float adc[4]; //ADC ports [volt] -% float local_position[3]; //tangent plane mapping into x,y,z [m] -% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] -% float attitude[3]; //pitch, roll, yaw [rad] -% float rotMatrix[9]; //unitvectors -% float actuator_control[4]; //unitvector -% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] -% float diff_pressure; - pressure difference in millibar -% float ind_airspeed; -% float true_airspeed; - -% Definition of the logged values -logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); -logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); -logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - -% First get length of one line -columns = length(logFormat); -lineLength = 0; - -for i=1:columns - lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; -end - - -if exist(filePath, 'file') - - fileInfo = dir(filePath); - fileSize = fileInfo.bytes; - - elements = int64(fileSize./(lineLength)); - - fid = fopen(filePath, 'r'); - offset = 0; - for i=1:columns - % using fread with a skip speeds up the import drastically, do not - % import the values one after the other - sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... - fid, ... - [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... - lineLength - logFormat{i}.bytes*logFormat{i}.array, ... - logFormat{i}.machineformat) ... - ); - offset = offset + logFormat{i}.bytes*logFormat{i}.array; - fseek(fid, offset,'bof'); - end - - % shot the flight time - time_us = sysvector.timestamp(end) - sysvector.timestamp(1); - time_s = time_us*1e-6; - time_m = time_s/60; - - % close the logfile - fclose(fid); - - disp(['end log2matlab conversion' char(10)]); -else - disp(['file: ' filePath ' does not exist' char(10)]); -end - -%% Plot GPS RAW measurements - -% Only plot GPS data if available -if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0 - figure('units','normalized','outerposition',[0 0 1 1]) - plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3)); -end - - -%% Plot optical flow trajectory - -flow_sz = size(sysvector.timestamp); -flow_elements = flow_sz(1); - -xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms] - - -%calc dt -dt = zeros(flow_elements,1); -for i = 1:flow_elements-1 - dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s] -end -dt(1,1) = mean(dt); - - -global_speed = zeros(flow_elements,3); - -%calc global speed (with rot matrix) -for i = 1:flow_elements - rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]'; - speedX = sysvector.optical_flow(i,3); - speedY = sysvector.optical_flow(i,4); - - relSpeed = [-speedY,speedX,0]; - global_speed(i,:) = relSpeed * rotM; -end - - - -px = zeros(flow_elements,1); -py = zeros(flow_elements,1); -distance = 0; - -last_vx = 0; -last_vy = 0; -elem_cnt = 0; - -% Very basic accumulation, stops on bad flow quality -for i = 1:flow_elements - if sysvector.optical_flow(i,6) > 5 - px(i,1) = global_speed(i,1)*dt(i,1); - py(i,1) = global_speed(i,2)*dt(i,1); - distance = distance + norm([px(i,1) py(i,1)]); - last_vx = px(i,1); - last_vy = py(i,1); - else - px(i,1) = last_vx; - py(i,1) = last_vy; - last_vx = last_vx*0.95; - last_vy = last_vy*0.95; - end -end - -px_sum = cumsum(px); -py_sum = cumsum(py); -time = cumsum(dt); - -figure() -set(gca, 'Units','normal'); - -plot(py_sum, px_sum, '-blue', 'LineWidth',2); -axis equal; -% set title and axis captions -xlabel('X position (meters)','fontsize',14) -ylabel('Y position (meters)','fontsize',14) -% mark begin and end -hold on -plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,... -'MarkerEdgeColor','k',... -'MarkerFaceColor','g',... -'MarkerSize',10) -hold on -plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,... -'MarkerEdgeColor','k',... -'MarkerFaceColor','b',... -'MarkerSize',10) -% add total length as annotation -set(gca,'fontsize',13); -legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60)); -title('Optical Flow Position Integration', 'fontsize', 15); - -figure() -plot(time, sysvector.optical_flow(:,5), 'blue'); -axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]); -xlabel('seconds','fontsize',14); -ylabel('m','fontsize',14); -set(gca,'fontsize',13); -title('Ultrasound Altitude', 'fontsize', 15); - - -figure() -plot(time, global_speed(:,2), 'red'); -hold on; -plot(time, global_speed(:,1), 'blue'); -legend('y velocity (m/s)', 'x velocity (m/s)'); -xlabel('seconds','fontsize',14); -ylabel('m/s','fontsize',14); -set(gca,'fontsize',13); -title('Optical Flow Velocity', 'fontsize', 15); diff --git a/ROMFS/px4fmu_default/mixers/FMU_AERT.mix b/ROMFS/px4fmu_default/mixers/FMU_AERT.mix deleted file mode 100644 index 75e82bb00..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_AERT.mix +++ /dev/null @@ -1,64 +0,0 @@ -Aileron/rudder/elevator/throttle mixer for PX4FMU -================================================== - -This file defines mixers suitable for controlling a fixed wing aircraft with -aileron, rudder, elevator and throttle controls using PX4FMU. The configuration -assumes the aileron servo(s) are connected to PX4FMU servo output 0, the -elevator to output 1, the rudder to output 2 and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -Aileron mixer -------------- -Two scalers total (output, roll). - -This mixer assumes that the aileron servos are set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -Elevator mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 - -Rudder mixer ------------- -Two scalers total (output, yaw). - -This mixer assumes that the rudder servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 2 10000 10000 0 -10000 10000 - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_default/mixers/FMU_AET.mix b/ROMFS/px4fmu_default/mixers/FMU_AET.mix deleted file mode 100644 index 20cb88b91..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_AET.mix +++ /dev/null @@ -1,60 +0,0 @@ -Aileron/elevator/throttle mixer for PX4FMU -================================================== - -This file defines mixers suitable for controlling a fixed wing aircraft with -aileron, elevator and throttle controls using PX4FMU. The configuration assumes -the aileron servo(s) are connected to PX4FMU servo output 0, the elevator to -output 1 and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -Aileron mixer -------------- -Two scalers total (output, roll). - -This mixer assumes that the aileron servos are set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -Alternatively, output 2 could be used as a second aileron servo output with -separate mixing. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -Elevator mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_default/mixers/FMU_Q.mix b/ROMFS/px4fmu_default/mixers/FMU_Q.mix deleted file mode 100644 index ebcb66b24..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_Q.mix +++ /dev/null @@ -1,52 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for Bormatec Camflyer Q - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -8000 -5000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_RET.mix b/ROMFS/px4fmu_default/mixers/FMU_RET.mix deleted file mode 100644 index 95beb8927..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_RET.mix +++ /dev/null @@ -1,53 +0,0 @@ -Rudder/elevator/throttle mixer for PX4FMU -========================================= - -This file defines mixers suitable for controlling a fixed wing aircraft with -rudder, elevator and throttle controls using PX4FMU. The configuration assumes -the rudder servo is connected to PX4FMU servo output 0, the elevator to output 1 -and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -Rudder mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the rudder servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -Elevator mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_default/mixers/FMU_X5.mix b/ROMFS/px4fmu_default/mixers/FMU_X5.mix deleted file mode 100644 index 9f81e1dc3..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_X5.mix +++ /dev/null @@ -1,50 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -3000 -5000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -3000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_delta.mix b/ROMFS/px4fmu_default/mixers/FMU_delta.mix deleted file mode 100644 index 981466704..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_delta.mix +++ /dev/null @@ -1,50 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 3000 5000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 - -M: 2 -O: 10000 10000 0 -10000 10000 -S: 0 0 5000 3000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 0 20000 -10000 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix deleted file mode 100644 index b5e38ce9e..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_hex_+.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the + configuration. All controls -are mixed 100%. - -R: 6+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix deleted file mode 100644 index 8e8d122ad..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_hex_x.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a hexacopter in the X configuration. All controls -are mixed 100%. - -R: 6x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix deleted file mode 100644 index 2cb70e814..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_octo_+.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a octocopter in the + configuration. All controls -are mixed 100%. - -R: 8+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix deleted file mode 100644 index edc71f013..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_octo_x.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a octocopter in the X configuration. All controls -are mixed 100%. - -R: 8x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_pass.mix b/ROMFS/px4fmu_default/mixers/FMU_pass.mix deleted file mode 100644 index e9a81f2bb..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_pass.mix +++ /dev/null @@ -1,23 +0,0 @@ -Passthrough mixer for PX4FMU -============================ - -This file defines passthrough mixers suitable for testing. - -Channel group 0, channels 0-3 are passed directly through to the outputs. - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 0 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 1 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 2 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - diff --git a/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix deleted file mode 100644 index dfdf1d58e..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_quad_+.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a quadrotor in the + configuration. All controls -are mixed 100%. - -R: 4+ 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix deleted file mode 100644 index 12a3bee20..000000000 --- a/ROMFS/px4fmu_default/mixers/FMU_quad_x.mix +++ /dev/null @@ -1,7 +0,0 @@ -Multirotor mixer for PX4FMU -=========================== - -This file defines a single mixer for a quadrotor in the X configuration. All controls -are mixed 100%. - -R: 4x 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_default/mixers/README b/ROMFS/px4fmu_default/mixers/README deleted file mode 100644 index 6649c53c2..000000000 --- a/ROMFS/px4fmu_default/mixers/README +++ /dev/null @@ -1,154 +0,0 @@ -PX4 mixer definitions -===================== - -Files in this directory implement example mixers that can be used as a basis -for customisation, or for general testing purposes. - -Mixer basics ------------- - -Mixers combine control values from various sources (control tasks, user inputs, -etc.) and produce output values suitable for controlling actuators; servos, -motors, switches and so on. - -An actuator derives its value from the combination of one or more control -values. Each of the control values is scaled according to the actuator's -configuration and then combined to produce the actuator value, which may then be -further scaled to suit the specific output type. - -Internally, all scaling is performed using floating point values. Inputs and -outputs are clamped to the range -1.0 to 1.0. - -control control control - | | | - v v v - scale scale scale - | | | - | v | - +-------> mix <------+ - | - scale - | - v - out - -Scaling -------- - -Basic scalers provide linear scaling of the input to the output. - -Each scaler allows the input value to be scaled independently for inputs -greater/less than zero. An offset can be applied to the output, and lower and -upper boundary constraints can be applied. Negative scaling factors cause the -output to be inverted (negative input produces positive output). - -Scaler pseudocode: - -if (input < 0) - output = (input * NEGATIVE_SCALE) + OFFSET -else - output = (input * POSITIVE_SCALE) + OFFSET - -if (output < LOWER_LIMIT) - output = LOWER_LIMIT -if (output > UPPER_LIMIT) - output = UPPER_LIMIT - -Syntax ------- - -Mixer definitions are text files; lines beginning with a single capital letter -followed by a colon are significant. All other lines are ignored, meaning that -explanatory text can be freely mixed with the definitions. - -Each file may define more than one mixer; the allocation of mixers to actuators -is specific to the device reading the mixer definition, and the number of -actuator outputs generated by a mixer is specific to the mixer. - -A mixer begins with a line of the form - - : - -The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a -multirotor mixer, etc. - -Null Mixer -.......... - -A null mixer consumes no controls and generates a single actuator output whose -value is always zero. Typically a null mixer is used as a placeholder in a -collection of mixers in order to achieve a specific pattern of actuator outputs. - -The null mixer definition has the form: - - Z: - -Simple Mixer -............ - -A simple mixer combines zero or more control inputs into a single actuator -output. Inputs are scaled, and the mixing function sums the result before -applying an output scaler. - -A simple mixer definition begins with: - - M: - O: <-ve scale> <+ve scale> - -If is zero, the sum is effectively zero and the mixer will -output a fixed value that is constrained by and . - -The second line defines the output scaler with scaler parameters as discussed -above. Whilst the calculations are performed as floating-point operations, the -values stored in the definition file are scaled by a factor of 10000; i.e. an -offset of -0.5 is encoded as -5000. - -The definition continues with entries describing the control -inputs and their scaling, in the form: - - S: <-ve scale> <+ve scale> - -The value identifies the control group from which the scaler will read, -and the value an offset within that group. These values are specific to -the device reading the mixer definition. - -When used to mix vehicle controls, mixer group zero is the vehicle attitude -control group, and index values zero through three are normally roll, pitch, -yaw and thrust respectively. - -The remaining fields on the line configure the control scaler with parameters as -discussed above. Whilst the calculations are performed as floating-point -operations, the values stored in the definition file are scaled by a factor of -10000; i.e. an offset of -0.5 is encoded as -5000. - -Multirotor Mixer -................ - -The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) -into a set of actuator outputs intended to drive motor speed controllers. - -The mixer definition is a single line of the form: - -R: - -The supported geometries include: - - 4x - quadrotor in X configuration - 4+ - quadrotor in + configuration - 6x - hexcopter in X configuration - 6+ - hexcopter in + configuration - 8x - octocopter in X configuration - 8+ - octocopter in + configuration - -Each of the roll, pitch and yaw scale values determine scaling of the roll, -pitch and yaw controls relative to the thrust control. Whilst the calculations -are performed as floating-point operations, the values stored in the definition -file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. - -Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the -thrust input ranges from 0.0 to 1.0. Output for each actuator is in the -range -1.0 to 1.0. - -In the case where an actuator saturates, all actuator values are rescaled so that -the saturating actuator is limited to 1.0. diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index a6d766511..b9ce1123f 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -5,7 +5,7 @@ # # Use the configuration's ROMFS. # -ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 374c0cfe9..bd324d7d0 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -5,7 +5,7 @@ # # Use the configuration's ROMFS. # -ROMFS_ROOT = $(PX4_BASE)/ROMFS/$(CONFIG) +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index c34382ae0..a2227b5c4 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -246,6 +246,9 @@ include $(PX4_MK_DIR)/nuttx.mk ################################################################################ ifneq ($(ROMFS_ROOT),) +ifeq ($(wildcard $(ROMFS_ROOT)),) +$(error ROMFS_ROOT specifies a directory that does not exist) +endif # # Note that there is no support for more than one root directory or constructing @@ -272,7 +275,7 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) # Generate the ROMFS image from the root $(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS) - @$(ECHO) %% generating $@ + @$(ECHO) "ROMFS: $@" $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) @@ -318,7 +321,7 @@ endef # Don't generate until modules have updated their command files $(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) - @$(ECHO) %% generating $@ + @$(ECHO) "CMDS: $@" $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include ' >> $@ $(Q) $(ECHO) '#include ' >> $@ diff --git a/makefiles/module.mk b/makefiles/module.mk index 154d37cc2..1db0f6fee 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -145,7 +145,6 @@ MODULE_COMMAND_FILES := $(addprefix $(WORK_DIR)/builtin_commands/COMMAND.,$(MODU $(MODULE_COMMAND_FILES): command = $(word 2,$(subst ., ,$(notdir $(@)))) $(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).* $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) - @$(ECHO) COMMAND: $(command) @$(REMOVE) -f $(exclude) @$(MKDIR) -p $(dir $@) $(Q) $(TOUCH) $@ -- cgit v1.2.3 From 8f1070bb42dbc597c3d2fa7bf9a5931896d651f9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 12:28:36 -0700 Subject: Fix a misleading comment about the tone_alarm timers. --- nuttx/configs/px4fmuv2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 286dedab0..5eae24477 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -309,9 +309,9 @@ /* * Tone alarm output */ -#define TONE_ALARM_TIMER 2 /* timer 3 */ -#define TONE_ALARM_CHANNEL 1 /* channel 3 */ -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_PULLUP|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) /************************************************************************************ * Public Data -- cgit v1.2.3 From 52bb5e561c2407937d80545c127e37da6d2c3a04 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 12:57:53 -0700 Subject: Fix memory sizing so that we get the extra 64K we promised. --- nuttx/arch/arm/src/stm32/stm32_allocateheap.c | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_allocateheap.c b/nuttx/arch/arm/src/stm32/stm32_allocateheap.c index 4b8707a2b..c53ff3137 100644 --- a/nuttx/arch/arm/src/stm32/stm32_allocateheap.c +++ b/nuttx/arch/arm/src/stm32/stm32_allocateheap.c @@ -118,19 +118,23 @@ # undef CONFIG_STM32_CCMEXCLUDE # define CONFIG_STM32_CCMEXCLUDE 1 -/* All members of the STM32F20xxx and STM32F40xxx families have 192Kb in three banks: +/* All members of the STM32F20xxx and STM32F40xxx families have 192KiB in three banks: * - * 1) 112Kb of System SRAM beginning at address 0x2000:0000 - * 2) 16Kb of System SRAM beginning at address 0x2001:c000 - * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * 1) 112KiB of System SRAM beginning at address 0x2000:0000 + * 2) 16KiB of System SRAM beginning at address 0x2001:c000 + * 3) 64KiB of CCM SRAM beginning at address 0x1000:0000 * - * As determined by ld.script, g_heapbase lies in the 112Kb memory + * The STM32F427/437 have an additional 64KiB of System SRAM beginning at + * address 0x2002:0000 for a total of 256KiB. + * + * As determined by ld.script, g_heapbase lies in the 112KiB memory * region and that extends to 0x2001:0000. But the first and second memory * regions are contiguous and treated as one in this logic that extends to - * 0x2002:0000. + * 0x2002:0000 (or 0x2003:0000 for the F427/F437). * - * As a complication, it appears that CCM SRAM cannot be used for DMA. So, if - * STM32 DMA is enabled, CCM SRAM should probably be excluded from the heap. + * As a complication, CCM SRAM cannot be used for DMA. So, if STM32 DMA is enabled, + * CCM SRAM should probably be excluded from the heap or the application must take + * extra care to ensure that DMA buffers are not allocated in CCM SRAM. * * In addition, external FSMC SRAM may be available. */ @@ -147,7 +151,11 @@ /* Set the end of system SRAM */ -# define SRAM1_END 0x20020000 +# if defined(CONFIG_STM32_STM32F427) +# define SRAM1_END 0x20030000 +# else +# define SRAM1_END 0x20020000 +# endif /* Set the range of CCM SRAM as well (although we may not use it) */ -- cgit v1.2.3 From d8e8e6cd209e688de51a800123e513f4885655cd Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 14:07:00 -0700 Subject: Fix alt function selector for tone_alarm GPIO. --- nuttx/configs/px4fmuv2/include/board.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 5eae24477..8158e618d 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -311,7 +311,7 @@ */ #define TONE_ALARM_TIMER 2 /* timer 2 */ #define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_PULLUP|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) /************************************************************************************ * Public Data -- cgit v1.2.3 From e7e35616c01119eae7a436c260d88cdc6fec6ce5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 15:47:34 -0700 Subject: Fix the way that tone_alarm idles the GPIO and make it idle safely for v2 boards. --- apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 8 ++++---- nuttx/configs/px4fmu/include/board.h | 1 + nuttx/configs/px4fmuv2/include/board.h | 3 ++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index baa652d8a..ac5511e60 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -494,7 +494,7 @@ ToneAlarm::init() return ret; /* configure the GPIO to the idle state */ - stm32_configgpio(GPIO_TONE_ALARM); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); @@ -606,6 +606,8 @@ ToneAlarm::start_note(unsigned note) rEGR = GTIM_EGR_UG; // force a reload of the period rCCER |= TONE_CCER; // enable the output + // configure the GPIO to enable timer output + stm32_configgpio(GPIO_TONE_ALARM); } void @@ -616,10 +618,8 @@ ToneAlarm::stop_note() /* * Make sure the GPIO is not driving the speaker. - * - * XXX this presumes PX4FMU and the onboard speaker driver FET. */ - stm32_gpiowrite(GPIO_TONE_ALARM, 0); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); } void diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 8ad56a4c6..0bc0b3021 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -326,6 +326,7 @@ */ #define TONE_ALARM_TIMER 3 /* timer 3 */ #define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) /************************************************************************************ diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 8158e618d..973aab0ce 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -311,7 +311,8 @@ */ #define TONE_ALARM_TIMER 2 /* timer 2 */ #define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) /************************************************************************************ * Public Data -- cgit v1.2.3 From d1a2e9a9c12ae5f4b52299f57c6704a03304b766 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 18:18:49 -0700 Subject: Fix the v2 RGB LED ID --- nuttx/configs/px4fmuv2/include/board.h | 2 +- src/device/rgbled/rgbled.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index 973aab0ce..fd8f78b80 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -283,7 +283,7 @@ * * Note that these are unshifted addresses. */ -#define PX4_I2C_OBDEV_LED 0x5a +#define PX4_I2C_OBDEV_LED 0x55 /* * SPI diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp index 923e0a14f..dc1e469c0 100644 --- a/src/device/rgbled/rgbled.cpp +++ b/src/device/rgbled/rgbled.cpp @@ -65,7 +65,7 @@ #define LED_ONTIME 120 #define LED_OFFTIME 120 -#define ADDR 0x55 /**< I2C adress of TCA62724FMG */ +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ #define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ #define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ -- cgit v1.2.3 From 29324cc97f06519a3e74b292ccd53474936afd5a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 18:33:32 -0700 Subject: Add the SoC chip and common directories to the NuttX-related include paths. --- makefiles/nuttx.mk | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index e86f1370b..5186dc3ef 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -63,7 +63,10 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # # Add directories from the NuttX export to the relevant search paths # -INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include +INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ + += $(NUTTX_EXPORT_DIR)arch/chip \ + += $(NUTTX_EXPORT_DIR)arch/common + LIB_DIRS += $(NUTTX_EXPORT_DIR)libs LIBS += -lapps -lnuttx LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \ -- cgit v1.2.3 From 706dcb6a53cc0163572541b856902616b30258ae Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 18:34:29 -0700 Subject: Move the FMU driver from the old universe to the new universe so that we can teach it about v2. --- apps/drivers/px4fmu/Makefile | 44 -- apps/drivers/px4fmu/fmu.cpp | 1084 ------------------------------------ makefiles/config_px4fmu_default.mk | 6 +- nuttx/configs/px4fmu/nsh/appconfig | 1 - src/device/px4fmu/fmu.cpp | 1084 ++++++++++++++++++++++++++++++++++++ src/device/px4fmu/module.mk | 6 + 6 files changed, 1095 insertions(+), 1130 deletions(-) delete mode 100644 apps/drivers/px4fmu/Makefile delete mode 100644 apps/drivers/px4fmu/fmu.cpp create mode 100644 src/device/px4fmu/fmu.cpp create mode 100644 src/device/px4fmu/module.mk diff --git a/apps/drivers/px4fmu/Makefile b/apps/drivers/px4fmu/Makefile deleted file mode 100644 index 7f7c2a732..000000000 --- a/apps/drivers/px4fmu/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. -# -############################################################################ - -# -# Interface driver for the PX4FMU board -# - -APPNAME = fmu -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp deleted file mode 100644 index e54724536..000000000 --- a/apps/drivers/px4fmu/fmu.cpp +++ /dev/null @@ -1,1084 +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 fmu.cpp - * - * Driver/configurator for the PX4 FMU multi-purpose port. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -class PX4FMU : public device::CDev -{ -public: - enum Mode { - MODE_2PWM, - MODE_4PWM, - MODE_NONE - }; - PX4FMU(); - virtual ~PX4FMU(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual ssize_t write(file *filp, const char *buffer, size_t len); - - virtual int init(); - - int set_mode(Mode mode); - - int set_pwm_alt_rate(unsigned rate); - int set_pwm_alt_channels(uint32_t channels); - -private: - static const unsigned _max_actuators = 4; - - Mode _mode; - unsigned _pwm_default_rate; - unsigned _pwm_alt_rate; - uint32_t _pwm_alt_rate_channels; - unsigned _current_update_rate; - int _task; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; - unsigned _num_outputs; - bool _primary_pwm_device; - - volatile bool _task_should_exit; - bool _armed; - - MixerGroup *_mixers; - - actuator_controls_s _controls; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main() __attribute__((noreturn)); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); - -}; - -const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -}; - -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); - -namespace -{ - -PX4FMU *g_fmu; - -} // namespace - -PX4FMU::PX4FMU() : - CDev("fmuservo", "/dev/px4fmu"), - _mode(MODE_NONE), - _pwm_default_rate(50), - _pwm_alt_rate(50), - _pwm_alt_rate_channels(0), - _current_update_rate(0), - _task(-1), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), - _t_actuators_effective(0), - _num_outputs(0), - _primary_pwm_device(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr) -{ - _debug_enabled = true; -} - -PX4FMU::~PX4FMU() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); - } - - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - g_fmu = nullptr; -} - -int -PX4FMU::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) - return ret; - - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } - - /* reset GPIOs */ - gpio_reset(); - - /* start the IO interface task */ - _task = task_spawn("fmuservo", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -void -PX4FMU::task_main_trampoline(int argc, char *argv[]) -{ - g_fmu->task_main(); -} - -int -PX4FMU::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: // multi-port with flow control lines as PWM - case MODE_4PWM: // multi-port as 4 PWM outs - debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); - - break; - - case MODE_NONE: - debug("MODE_NONE"); - - _pwm_default_rate = 10; /* artificially reduced output rate */ - _pwm_alt_rate = 10; - _pwm_alt_rate_channels = 0; - - /* disable servo outputs - no need to set rates */ - up_pwm_servo_deinit(); - - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - -int -PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) -{ - debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); - - for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < _max_actuators; group++) { - - // get the channel mask for this rate group - uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) - continue; - - // all channels in the group must be either default or alt-rate - uint32_t alt = rate_map & mask; - - if (pass == 0) { - // preflight - if ((alt != 0) && (alt != mask)) { - warn("rate group %u mask %x bad overlap %x", group, mask, alt); - // not a legal map, bail - return -EINVAL; - } - } else { - // set it - errors here are unexpected - if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { - warn("rate group set alt failed"); - return -EINVAL; - } - } else { - if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { - warn("rate group set default failed"); - return -EINVAL; - } - } - } - } - } - _pwm_alt_rate_channels = rate_map; - _pwm_default_rate = default_rate; - _pwm_alt_rate = alt_rate; - - return OK; -} - -int -PX4FMU::set_pwm_alt_rate(unsigned rate) -{ - return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); -} - -int -PX4FMU::set_pwm_alt_channels(uint32_t channels) -{ - return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); -} - -void -PX4FMU::task_main() -{ - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - /* force a reset of the update rate */ - _current_update_rate = 0; - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; - - // rc input, published to ORB - struct rc_input_values rc_in; - orb_advert_t to_input_rc = 0; - - memset(&rc_in, 0, sizeof(rc_in)); - rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; - - log("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - /* - * Adjust actuator topic update rate to keep up with - * the highest servo update rate configured. - * - * We always mix at max rate; some channels may update slower. - */ - unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - if (_current_update_rate != max_rate) { - _current_update_rate = max_rate; - int update_rate_in_ms = int(1000 / _current_update_rate); - - /* reject faster than 500 Hz updates */ - if (update_rate_in_ms < 2) { - update_rate_in_ms = 2; - } - /* reject slower than 10 Hz updates */ - if (update_rate_in_ms > 100) { - update_rate_in_ms = 100; - } - - debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); - - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; - } - - /* sleep waiting for data, stopping to check for PPM - * input at 100Hz */ - int ret = ::poll(&fds[0], 2, 10); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - usleep(1000000); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } - - /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); - } - - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - - /* update PWM servo armed status if armed and not locked down */ - up_pwm_servo_arm(aa.armed && !aa.lockdown); - } - - // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { - // we have a new PPM frame. Publish it. - rc_in.channel_count = ppm_decoded_channels; - if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { - rc_in.channel_count = RC_INPUT_MAX_CHANNELS; - } - for (uint8_t i=0; icontrol[control_index]; - return 0; -} - -int -PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret; - - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - - /* try it as a GPIO ioctl first */ - ret = gpio_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) - return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch (_mode) { - case MODE_2PWM: - case MODE_4PWM: - ret = pwm_ioctl(filp, cmd, arg); - break; - - default: - debug("not in a PWM mode"); - break; - } - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) - ret = CDev::ioctl(filp, cmd, arg); - - return ret; -} - -int -PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); - break; - - case PWM_SERVO_SELECT_UPDATE_RATE: - ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); - break; - - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(0): - case PWM_SERVO_SET(1): - if (arg < 2100) { - up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): - *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); - break; - - case PWM_SERVO_GET_RATEGROUP(0): - case PWM_SERVO_GET_RATEGROUP(1): - case PWM_SERVO_GET_RATEGROUP(2): - case PWM_SERVO_GET_RATEGROUP(3): - *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); - break; - - case PWM_SERVO_GET_COUNT: - case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - - } else { - *(unsigned *)arg = 2; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - _mixers->add_mixer(mixer); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -/* - this implements PWM output via a write() method, for compatibility - with px4io - */ -ssize_t -PX4FMU::write(file *filp, const char *buffer, size_t len) -{ - unsigned count = len / 2; - uint16_t values[4]; - - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; - } - - // allow for misaligned values - memcpy(values, buffer, count*2); - - for (uint8_t i=0; iioctl(0, GPIO_RESET, 0); - - gpio_bits = 0; - servo_mode = PX4FMU::MODE_NONE; - - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = PX4FMU::MODE_4PWM; - break; - - case PORT_GPIO_AND_SERIAL: - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = PX4FMU::MODE_2PWM; - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = PX4FMU::MODE_2PWM; - break; - } - - /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) - g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - - /* (re)set the PWM output mode */ - g_fmu->set_mode(servo_mode); - - return OK; -} - -int -fmu_start(void) -{ - int ret = OK; - - if (g_fmu == nullptr) { - - g_fmu = new PX4FMU; - - if (g_fmu == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_fmu->init(); - - if (ret != OK) { - delete g_fmu; - g_fmu = nullptr; - } - } - } - - return ret; -} - -void -test(void) -{ - int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - - if (fd < 0) - errx(1, "open fail"); - - if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); - - close(fd); - - exit(0); -} - -void -fake(int argc, char *argv[]) -{ - if (argc < 5) - errx(1, "fmu fake (values -100 .. 100)"); - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle < 0) - errx(1, "advertise failed"); - - actuator_armed_s aa; - - aa.armed = true; - aa.lockdown = false; - - handle = orb_advertise(ORB_ID(actuator_armed), &aa); - - if (handle < 0) - errx(1, "advertise failed 2"); - - exit(0); -} - -} // namespace - -extern "C" __EXPORT int fmu_main(int argc, char *argv[]); - -int -fmu_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNSET; - const char *verb = argv[1]; - - if (fmu_start() != OK) - errx(1, "failed to start the FMU driver"); - - /* - * Mode switches. - */ - if (!strcmp(verb, "mode_gpio")) { - new_mode = PORT_FULL_GPIO; - - } else if (!strcmp(verb, "mode_serial")) { - new_mode = PORT_FULL_SERIAL; - - } else if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT_FULL_PWM; - - } else if (!strcmp(verb, "mode_gpio_serial")) { - new_mode = PORT_GPIO_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_serial")) { - new_mode = PORT_PWM_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_gpio")) { - new_mode = PORT_PWM_AND_GPIO; - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNSET) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) - return OK; - - /* switch modes */ - int ret = fmu_new_mode(new_mode); - exit(ret == OK ? 0 : 1); - } - - if (!strcmp(verb, "test")) - test(); - - if (!strcmp(verb, "fake")) - fake(argc - 1, argv + 1); - - fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); - exit(1); -} diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index b9ce1123f..39b47d817 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -7,6 +7,11 @@ # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +# +# Board support modules +# +MODULES += device/px4fmu + # # Transitional support - add commands from the NuttX export archive. # @@ -34,7 +39,6 @@ BUILTIN_COMMANDS := \ $(call _B, eeprom, , 4096, eeprom_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, fmu, , 2048, fmu_main ) \ $(call _B, gps, , 2048, gps_main ) \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hmc5883, , 4096, hmc5883_main ) \ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312..d642b4692 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -123,7 +123,6 @@ CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc -CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps CONFIGURED_APPS += drivers/mb12xx diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp new file mode 100644 index 000000000..e54724536 --- /dev/null +++ b/src/device/px4fmu/fmu.cpp @@ -0,0 +1,1084 @@ +/**************************************************************************** + * + * 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 fmu.cpp + * + * Driver/configurator for the PX4 FMU multi-purpose port. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +class PX4FMU : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_NONE + }; + PX4FMU(); + virtual ~PX4FMU(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + + virtual int init(); + + int set_mode(Mode mode); + + int set_pwm_alt_rate(unsigned rate); + int set_pwm_alt_channels(uint32_t channels); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + unsigned _pwm_default_rate; + unsigned _pwm_alt_rate; + uint32_t _pwm_alt_rate_channels; + unsigned _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + +}; + +const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +}; + +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); + +namespace +{ + +PX4FMU *g_fmu; + +} // namespace + +PX4FMU::PX4FMU() : + CDev("fmuservo", "/dev/px4fmu"), + _mode(MODE_NONE), + _pwm_default_rate(50), + _pwm_alt_rate(50), + _pwm_alt_rate_channels(0), + _current_update_rate(0), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _t_actuators_effective(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +PX4FMU::~PX4FMU() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_fmu = nullptr; +} + +int +PX4FMU::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + gpio_reset(); + + /* start the IO interface task */ + _task = task_spawn("fmuservo", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +PX4FMU::task_main_trampoline(int argc, char *argv[]) +{ + g_fmu->task_main(); +} + +int +PX4FMU::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: // multi-port with flow control lines as PWM + case MODE_4PWM: // multi-port as 4 PWM outs + debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_NONE: + debug("MODE_NONE"); + + _pwm_default_rate = 10; /* artificially reduced output rate */ + _pwm_alt_rate = 10; + _pwm_alt_rate_channels = 0; + + /* disable servo outputs - no need to set rates */ + up_pwm_servo_deinit(); + + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) +{ + debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < _max_actuators; group++) { + + // get the channel mask for this rate group + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + // all channels in the group must be either default or alt-rate + uint32_t alt = rate_map & mask; + + if (pass == 0) { + // preflight + if ((alt != 0) && (alt != mask)) { + warn("rate group %u mask %x bad overlap %x", group, mask, alt); + // not a legal map, bail + return -EINVAL; + } + } else { + // set it - errors here are unexpected + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { + warn("rate group set alt failed"); + return -EINVAL; + } + } else { + if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { + warn("rate group set default failed"); + return -EINVAL; + } + } + } + } + } + _pwm_alt_rate_channels = rate_map; + _pwm_default_rate = default_rate; + _pwm_alt_rate = alt_rate; + + return OK; +} + +int +PX4FMU::set_pwm_alt_rate(unsigned rate) +{ + return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); +} + +int +PX4FMU::set_pwm_alt_channels(uint32_t channels) +{ + return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); +} + +void +PX4FMU::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } + + debug("adjusted actuator update interval to %ums", update_rate_in_ms); + orb_set_interval(_t_actuators, update_rate_in_ms); + + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } + + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + + /* output to the servo */ + up_pwm_servo_set(i, outputs.output[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status if armed and not locked down */ + up_pwm_servo_arm(aa.armed && !aa.lockdown); + } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; icontrol[control_index]; + return 0; +} + +int +PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); + + /* try it as a GPIO ioctl first */ + ret = gpio_ioctl(filp, cmd, arg); + + if (ret != -ENOTTY) + return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch (_mode) { + case MODE_2PWM: + case MODE_4PWM: + ret = pwm_ioctl(filp, cmd, arg); + break; + + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): + *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); + break; + + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + + } else { + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +PX4FMU::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[4]; + + if (count > 4) { + // we only have 4 PWM outputs on the FMU + count = 4; + } + + // allow for misaligned values + memcpy(values, buffer, count*2); + + for (uint8_t i=0; iioctl(0, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = PX4FMU::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + break; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_fmu->set_mode(servo_mode); + + return OK; +} + +int +fmu_start(void) +{ + int ret = OK; + + if (g_fmu == nullptr) { + + g_fmu = new PX4FMU; + + if (g_fmu == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_fmu->init(); + + if (ret != OK) { + delete g_fmu; + g_fmu = nullptr; + } + } + } + + return ret; +} + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + + close(fd); + + exit(0); +} + +void +fake(int argc, char *argv[]) +{ + if (argc < 5) + errx(1, "fmu fake (values -100 .. 100)"); + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) + errx(1, "advertise failed"); + + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); + + exit(0); +} + +} // namespace + +extern "C" __EXPORT int fmu_main(int argc, char *argv[]); + +int +fmu_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + const char *verb = argv[1]; + + if (fmu_start() != OK) + errx(1, "failed to start the FMU driver"); + + /* + * Mode switches. + */ + if (!strcmp(verb, "mode_gpio")) { + new_mode = PORT_FULL_GPIO; + + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + + } else if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT_FULL_PWM; + + } else if (!strcmp(verb, "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + int ret = fmu_new_mode(new_mode); + exit(ret == OK ? 0 : 1); + } + + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); + + fprintf(stderr, "FMU: unrecognised command, try:\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + exit(1); +} diff --git a/src/device/px4fmu/module.mk b/src/device/px4fmu/module.mk new file mode 100644 index 000000000..05bc7a5b3 --- /dev/null +++ b/src/device/px4fmu/module.mk @@ -0,0 +1,6 @@ +# +# Interface driver for the PX4FMU board +# + +MODULE_COMMAND = fmu +SRCS = fmu.cpp -- cgit v1.2.3 From c355275669378c0d6f2e372afa370446525c66ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 19:20:08 -0700 Subject: Make the 'fmu' command build for v2. Should be enough to get the FMU-side PWM outputs working, but untested. --- apps/drivers/boards/px4fmuv2/px4fmu_internal.h | 17 +++ apps/drivers/drv_gpio.h | 27 +++- makefiles/config_px4fmuv2_default.mk | 5 +- src/device/px4fmu/fmu.cpp | 194 ++++++++++++++++++++----- 4 files changed, 201 insertions(+), 42 deletions(-) diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h index 001b23cb2..235b193f3 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -71,6 +71,23 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index 2fa6d8b8e..d21fc5c33 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -42,11 +42,6 @@ #include -#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 -#warning No GPIOs on this board. -#define GPIO_DEVICE_PATH "/dev/null" -#endif - #ifdef CONFIG_ARCH_BOARD_PX4FMU /* * PX4FMU GPIO numbers. @@ -72,6 +67,28 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +/* + * PX4FMUv2 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +/** + * Default GPIO device - other devices may also support this protocol if + * they also export GPIO-like things. This is always the GPIOs on the + * main board. + */ +# define GPIO_DEVICE_PATH "/dev/px4fmu" + +#endif + #ifdef CONFIG_ARCH_BOARD_PX4IO /* * PX4IO GPIO numbers. diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index bd324d7d0..11ca5227e 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,8 +10,9 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += device/rgbled -MODULES += device/lsm303d +MODULES += device/lsm303d +MODULES += device/px4fmu +MODULES += device/rgbled # # Transitional support - add commands from the NuttX export archive. diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp index e54724536..7b8ca9bbe 100644 --- a/src/device/px4fmu/fmu.cpp +++ b/src/device/px4fmu/fmu.cpp @@ -34,7 +34,7 @@ /** * @file fmu.cpp * - * Driver/configurator for the PX4 FMU multi-purpose port. + * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ #include @@ -57,9 +57,18 @@ #include #include #include -#include #include +#if defined(CONFIG_ARCH_BOARD_PX4FMU) +# include +# define FMU_HAVE_PPM +#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +# include +# undef FMU_HAVE_PPM +#else +# error Unrecognised FMU board. +#endif + #include #include #include @@ -71,15 +80,18 @@ #include #include -#include +#ifdef FMU_HAVE_PPM +# include +#endif class PX4FMU : public device::CDev { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -146,6 +158,7 @@ private: }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -154,6 +167,15 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -271,9 +293,8 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { - case MODE_2PWM: // multi-port with flow control lines as PWM - case MODE_4PWM: // multi-port as 4 PWM outs - debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + case MODE_2PWM: // v1 multi-port with flow control lines as PWM + debug("MODE_2PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -281,7 +302,35 @@ PX4FMU::set_mode(Mode mode) _pwm_alt_rate_channels = 0; /* XXX magic numbers */ - up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + up_pwm_servo_init(0x3); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_4PWM: // v1 multi-port as 4 PWM outs + debug("MODE_4PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_6PWM: // v2 PWMs as 6 PWM outs + debug("MODE_6PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3f); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -399,14 +448,14 @@ PX4FMU::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; - +#ifdef FMU_HAVE_PPM // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; +#endif log("starting"); @@ -460,6 +509,23 @@ PX4FMU::task_main() /* can we mix? */ if (_mixers != nullptr) { + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; + } + /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); outputs.timestamp = hrt_absolute_time(); @@ -508,6 +574,7 @@ PX4FMU::task_main() up_pwm_servo_arm(aa.armed && !aa.lockdown); } +#ifdef FMU_HAVE_PPM // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. @@ -527,6 +594,8 @@ PX4FMU::task_main() orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } +#endif + } ::close(_t_actuators); @@ -575,6 +644,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -614,16 +684,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; - case PWM_SERVO_SET(2): + case PWM_SERVO_SET(5): + case PWM_SERVO_SET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_SET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): + case PWM_SERVO_SET(0): if (arg < 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { @@ -632,16 +710,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; - case PWM_SERVO_GET(2): + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_GET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); break; @@ -649,16 +735,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(1): case PWM_SERVO_GET_RATEGROUP(2): case PWM_SERVO_GET_RATEGROUP(3): + case PWM_SERVO_GET_RATEGROUP(4): + case PWM_SERVO_GET_RATEGROUP(5): *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { + switch (_mode) { + case MODE_6PWM: + *(unsigned *)arg = 6; + break; + case MODE_4PWM: *(unsigned *)arg = 4; - - } else { + break; + case MODE_2PWM: *(unsigned *)arg = 2; + break; + default: + ret = -EINVAL; + break; } break; @@ -734,17 +830,17 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[4]; + uint16_t values[6]; - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; + if (count > 6) { + // we have at most 6 outputs + count = 6; } // allow for misaligned values - memcpy(values, buffer, count*2); + memcpy(values, buffer, count * 2); - for (uint8_t i=0; i Date: Sat, 6 Apr 2013 22:46:50 -0700 Subject: Add GPIO driver access to the power supply control/monitoring GPIOs for FMUv2 --- apps/drivers/boards/px4fmuv2/px4fmu_init.c | 19 ++++++++++++------- apps/drivers/boards/px4fmuv2/px4fmu_internal.h | 6 ++++++ apps/drivers/drv_gpio.h | 4 ++++ src/device/px4fmu/fmu.cpp | 15 ++++++++++++--- 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/apps/drivers/boards/px4fmuv2/px4fmu_init.c index c9364c2a4..1d99f15bf 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/apps/drivers/boards/px4fmuv2/px4fmu_init.c @@ -145,7 +145,17 @@ __EXPORT int matherr(struct exception *e) __EXPORT int nsh_archinitialize(void) { - int result; + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + stm32_configgpio(GPIO_ADC1_IN12); + + /* configure power supply control pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); /* configure the high-resolution time/callout interface */ hrt_init(); @@ -201,7 +211,7 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the FRAM */ message("[boot] Initializing SPI port 2\n"); - spi2 = up_spiinitialize(3); + spi2 = up_spiinitialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); @@ -215,10 +225,5 @@ __EXPORT int nsh_archinitialize(void) //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); - /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - stm32_configgpio(GPIO_ADC1_IN12); - return OK; } diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h index 235b193f3..cc4e9529d 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -88,6 +88,12 @@ __BEGIN_DECLS #define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) #define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index d21fc5c33..a4c59d218 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -80,6 +80,10 @@ # define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ # define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - VDD_5V_PERIPH_EN */ +# define GPIO_5V_HIPOWER_OC (1<<7) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<8) /**< PE15 - !VDD_5V_PERIPH_OC */ + /** * Default GPIO device - other devices may also support this protocol if * they also export GPIO-like things. This is always the GPIOs on the diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp index 7b8ca9bbe..d3865f053 100644 --- a/src/device/px4fmu/fmu.cpp +++ b/src/device/px4fmu/fmu.cpp @@ -175,6 +175,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {GPIO_5V_HIPOWER_OC, 0, 0}, + {GPIO_5V_PERIPH_OC, 0, 0}, #endif }; @@ -850,10 +853,16 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } #if defined(CONFIG_ARCH_BOARD_PX4FMU) /* if we have a GPIO direction control, set it to zero (input) */ -- cgit v1.2.3 From 2557f0d2de80e2df690b40b60f8ec79de799657e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 23:04:32 -0700 Subject: Rename the 'device' directory back to 'drivers', it's less confusing that way. Move the fmuv2 board driver out into the new world. --- apps/drivers/boards/px4fmuv2/Makefile | 41 - apps/drivers/boards/px4fmuv2/px4fmu_can.c | 144 --- apps/drivers/boards/px4fmuv2/px4fmu_init.c | 229 ---- apps/drivers/boards/px4fmuv2/px4fmu_internal.h | 129 --- apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 105 -- apps/drivers/boards/px4fmuv2/px4fmu_spi.c | 141 --- apps/drivers/boards/px4fmuv2/px4fmu_usb.c | 108 -- makefiles/config_px4fmu_default.mk | 2 +- makefiles/config_px4fmuv2_default.mk | 7 +- src/device/lsm303d/lsm303d.cpp | 1290 ----------------------- src/device/lsm303d/module.mk | 6 - src/device/px4fmu/fmu.cpp | 1217 --------------------- src/device/px4fmu/module.mk | 6 - src/device/rgbled/module.mk | 6 - src/device/rgbled/rgbled.cpp | 497 --------- src/drivers/boards/px4fmuv2/module.mk | 9 + src/drivers/boards/px4fmuv2/px4fmu_can.c | 144 +++ src/drivers/boards/px4fmuv2/px4fmu_init.c | 229 ++++ src/drivers/boards/px4fmuv2/px4fmu_internal.h | 129 +++ src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 105 ++ src/drivers/boards/px4fmuv2/px4fmu_spi.c | 141 +++ src/drivers/boards/px4fmuv2/px4fmu_usb.c | 108 ++ src/drivers/lsm303d/lsm303d.cpp | 1290 +++++++++++++++++++++++ src/drivers/lsm303d/module.mk | 6 + src/drivers/px4fmu/fmu.cpp | 1217 +++++++++++++++++++++ src/drivers/px4fmu/module.mk | 6 + src/drivers/rgbled/module.mk | 6 + src/drivers/rgbled/rgbled.cpp | 497 +++++++++ 28 files changed, 3892 insertions(+), 3923 deletions(-) delete mode 100644 apps/drivers/boards/px4fmuv2/Makefile delete mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_can.c delete mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_init.c delete mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_internal.h delete mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c delete mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_spi.c delete mode 100644 apps/drivers/boards/px4fmuv2/px4fmu_usb.c delete mode 100644 src/device/lsm303d/lsm303d.cpp delete mode 100644 src/device/lsm303d/module.mk delete mode 100644 src/device/px4fmu/fmu.cpp delete mode 100644 src/device/px4fmu/module.mk delete mode 100644 src/device/rgbled/module.mk delete mode 100644 src/device/rgbled/rgbled.cpp create mode 100644 src/drivers/boards/px4fmuv2/module.mk create mode 100644 src/drivers/boards/px4fmuv2/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmuv2/px4fmu_init.c create mode 100644 src/drivers/boards/px4fmuv2/px4fmu_internal.h create mode 100644 src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmuv2/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmuv2/px4fmu_usb.c create mode 100644 src/drivers/lsm303d/lsm303d.cpp create mode 100644 src/drivers/lsm303d/module.mk create mode 100644 src/drivers/px4fmu/fmu.cpp create mode 100644 src/drivers/px4fmu/module.mk create mode 100644 src/drivers/rgbled/module.mk create mode 100644 src/drivers/rgbled/rgbled.cpp diff --git a/apps/drivers/boards/px4fmuv2/Makefile b/apps/drivers/boards/px4fmuv2/Makefile deleted file mode 100644 index 9c04a8c42..000000000 --- a/apps/drivers/boards/px4fmuv2/Makefile +++ /dev/null @@ -1,41 +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. -# -############################################################################ - -# -# Board-specific startup code for the PX4FMUv2 -# - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common -LIBNAME = brd_px4fmuv2 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_can.c b/apps/drivers/boards/px4fmuv2/px4fmu_can.c deleted file mode 100644 index 86ba183b8..000000000 --- a/apps/drivers/boards/px4fmuv2/px4fmu_can.c +++ /dev/null @@ -1,144 +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 px4fmu_can.c - * - * Board-specific CAN functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include - -#include -#include - -#include "chip.h" -#include "up_arch.h" - -#include "stm32.h" -#include "stm32_can.h" -#include "px4fmu_internal.h" - -#ifdef CONFIG_CAN - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ -/* Configuration ********************************************************************/ - -#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) -# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." -# undef CONFIG_STM32_CAN2 -#endif - -#ifdef CONFIG_STM32_CAN1 -# define CAN_PORT 1 -#else -# define CAN_PORT 2 -#endif - -/* Debug ***************************************************************************/ -/* Non-standard debug that may be enabled just for testing CAN */ - -#ifdef CONFIG_DEBUG_CAN -# define candbg dbg -# define canvdbg vdbg -# define canlldbg lldbg -# define canllvdbg llvdbg -#else -# define candbg(x...) -# define canvdbg(x...) -# define canlldbg(x...) -# define canllvdbg(x...) -#endif - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: can_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/can. - * - ************************************************************************************/ - -int can_devinit(void) -{ - static bool initialized = false; - struct can_dev_s *can; - int ret; - - /* Check if we have already initialized */ - - if (!initialized) { - /* Call stm32_caninitialize() to get an instance of the CAN interface */ - - can = stm32_caninitialize(CAN_PORT); - - if (can == NULL) { - candbg("ERROR: Failed to get CAN interface\n"); - return -ENODEV; - } - - /* Register the CAN driver at "/dev/can0" */ - - ret = can_register("/dev/can0", can); - - if (ret < 0) { - candbg("ERROR: can_register failed: %d\n", ret); - return ret; - } - - /* Now we are initialized */ - - initialized = true; - } - - return OK; -} - -#endif \ No newline at end of file diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/apps/drivers/boards/px4fmuv2/px4fmu_init.c deleted file mode 100644 index 1d99f15bf..000000000 --- a/apps/drivers/boards/px4fmuv2/px4fmu_init.c +++ /dev/null @@ -1,229 +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 px4fmu_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "stm32_internal.h" -#include "px4fmu_internal.h" -#include "stm32_uart.h" - -#include - -#include -#include - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - /* configure SPI interfaces */ - stm32_spiinitialize(); - - /* configure LEDs */ - up_ledinit(); -} - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -static struct spi_dev_s *spi1; -static struct spi_dev_s *spi2; - -#include - -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif - -__EXPORT int nsh_archinitialize(void) -{ - - /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - stm32_configgpio(GPIO_ADC1_IN12); - - /* configure power supply control pins */ - stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); - stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); - stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); - - /* configure the high-resolution time/callout interface */ - hrt_init(); - - /* configure CPU load estimation */ -#ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); -#endif - - /* set up the serial DMA polling */ - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - - // initial LED state - // XXX need to make this work again -// drv_led_start(); - up_ledoff(LED_AMBER); - - /* Configure SPI-based devices */ - - spi1 = up_spiinitialize(1); - - if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - // Default SPI1 to 1MHz and de-assert the known chip selects. - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); - SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port 1\r\n"); - - /* Get the SPI port for the FRAM */ - - message("[boot] Initializing SPI port 2\n"); - spi2 = up_spiinitialize(2); - - if (!spi2) { - message("[boot] FAILED to initialize SPI port 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - message("[boot] Successfully initialized SPI port 2\n"); - - /* XXX need a driver to bind the FRAM to */ - - //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); - - return OK; -} diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h deleted file mode 100644 index cc4e9529d..000000000 --- a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h +++ /dev/null @@ -1,129 +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 px4fmu_internal.h - * - * PX4FMU internal definitions - */ - -#pragma once - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -__BEGIN_DECLS - -/* these headers are not C++ safe */ -#include - - -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/* PX4FMU GPIOs ***********************************************************************************/ -/* LEDs */ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) - -/* External interrupts */ - -/* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) - -/* User GPIOs - * - * GPIO0-5 are the PWM servo outputs. - */ -#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) -#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) -#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) -#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) -#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) -#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) -#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) -#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) -#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) -#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) - -/* Power supply control and monitoring GPIOs */ -#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) -#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) -#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) -#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) - -/* USB OTG FS - * - * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) - */ -#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) - -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - -#ifndef __ASSEMBLY__ - -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - -/**************************************************************************************************** - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ****************************************************************************************************/ - -extern void stm32_spiinitialize(void); - -#endif /* __ASSEMBLY__ */ - -__END_DECLS diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c deleted file mode 100644 index 14e4052be..000000000 --- a/apps/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ /dev/null @@ -1,105 +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 px4fmu_pwm_servo.c - * - * Configuration data for the stm32 pwm_servo driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include - -#include - -#include -#include - -#include -#include -#include - -__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { - { - .base = STM32_TIM1_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB2ENR_TIM1EN, - .clock_freq = STM32_APB2_TIM1_CLKIN - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN - } -}; - -__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { - { - .gpio = GPIO_TIM1_CH4OUT, - .timer_index = 0, - .timer_channel = 4, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM1_CH3OUT, - .timer_index = 0, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM1_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM1_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH2OUT, - .timer_index = 1, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH3OUT, - .timer_index = 1, - .timer_channel = 3, - .default_value = 1000, - } -}; diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_spi.c b/apps/drivers/boards/px4fmuv2/px4fmu_spi.c deleted file mode 100644 index f90f25071..000000000 --- a/apps/drivers/boards/px4fmuv2/px4fmu_spi.c +++ /dev/null @@ -1,141 +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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" -#include "px4fmu_internal.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void weak_function stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); -#endif - -#ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); -#endif -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - break; - - case PX4_SPIDEV_ACCEL_MAG: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - return SPI_STATUS_PRESENT; -} - - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_usb.c b/apps/drivers/boards/px4fmuv2/px4fmu_usb.c deleted file mode 100644 index b0b669fbe..000000000 --- a/apps/drivers/boards/px4fmuv2/px4fmu_usb.c +++ /dev/null @@ -1,108 +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 px4fmu_usb.c - * - * Board-specific USB functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "stm32_internal.h" -#include "px4fmu_internal.h" - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_usbinitialize - * - * Description: - * Called to setup USB-related GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_usbinitialize(void) -{ - /* The OTG FS has an internal soft pull-up */ - - /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ - -#ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); - /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); - */ -#endif -} - -/************************************************************************************ - * Name: stm32_usbsuspend - * - * Description: - * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is - * used. This function is called whenever the USB enters or leaves suspend mode. - * This is an opportunity for the board logic to shutdown clocks, power, etc. - * while the USB is suspended. - * - ************************************************************************************/ - -__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) -{ - ulldbg("resume: %d\n", resume); -} - diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 39b47d817..1a6c91b26 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -10,7 +10,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += device/px4fmu +MODULES += drivers/px4fmu # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 11ca5227e..d296c5379 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,9 +10,10 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += device/lsm303d -MODULES += device/px4fmu -MODULES += device/rgbled +MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/lsm303d +MODULES += drivers/px4fmu +MODULES += drivers/rgbled # # Transitional support - add commands from the NuttX export archive. diff --git a/src/device/lsm303d/lsm303d.cpp b/src/device/lsm303d/lsm303d.cpp deleted file mode 100644 index 32030a1f7..000000000 --- a/src/device/lsm303d/lsm303d.cpp +++ /dev/null @@ -1,1290 +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 lsm303d.cpp - * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - - - -/* register addresses: A: accel, M: mag, T: temp */ -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 - -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D - -#define ADDR_OUT_TEMP_A 0x26 -#define ADDR_STATUS_A 0x27 -#define ADDR_OUT_X_L_A 0x28 -#define ADDR_OUT_X_H_A 0x29 -#define ADDR_OUT_Y_L_A 0x2A -#define ADDR_OUT_Y_H_A 0x2B -#define ADDR_OUT_Z_L_A 0x2C -#define ADDR_OUT_Z_H_A 0x2D - -#define ADDR_CTRL_REG0 0x1F -#define ADDR_CTRL_REG1 0x20 -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define ADDR_CTRL_REG5 0x24 -#define ADDR_CTRL_REG7 0x26 - -#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) - -#define REG1_CONT_UPDATE_A (0<<3) -#define REG1_Z_ENABLE_A (1<<2) -#define REG1_Y_ENABLE_A (1<<1) -#define REG1_X_ENABLE_A (1<<0) - -#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) -#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) -#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) -#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) - -#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) -#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) -#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) -#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) -#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) - -#define REG5_ENABLE_T (1<<7) - -#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) -#define REG5_RES_LOW_M ((0<<6) | (0<<5)) - -#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) -#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) -#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) -#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) -#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) -#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) -#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) - -#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) -#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) -#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) -#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) - -#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) - - -#define INT_CTRL_M 0x12 -#define INT_SRC_M 0x13 - - -extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } - - -class LSM303D_mag; - -class LSM303D : public device::SPI -{ -public: - LSM303D(int bus, const char* path, spi_dev_e device); - virtual ~LSM303D(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - - friend class LSM303D_mag; - - virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); - -private: - - LSM303D_mag *_mag; - - struct hrt_call _accel_call; - struct hrt_call _mag_call; - - unsigned _call_accel_interval; - unsigned _call_mag_interval; - - unsigned _num_accel_reports; - volatile unsigned _next_accel_report; - volatile unsigned _oldest_accel_report; - struct accel_report *_accel_reports; - - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _num_mag_reports; - volatile unsigned _next_mag_report; - volatile unsigned _oldest_mag_report; - struct mag_report *_mag_reports; - - struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; - orb_advert_t _mag_topic; - - unsigned _current_accel_rate; - unsigned _current_accel_range; - - unsigned _current_mag_rate; - unsigned _current_mag_range; - - perf_counter_t _accel_sample_perf; - perf_counter_t _mag_sample_perf; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); - - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - void measure(); - - /** - * Fetch mag measurements from the sensor and update the report ring. - */ - void mag_measure(); - - /** - * Read a register from the LSM303D - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the LSM303D - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the LSM303D - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Set the LSM303D measurement range. - * - * @param max_dps The measurement range is set to permit reading at least - * this rate in degrees per second. - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_range(unsigned max_dps); - - /** - * Set the LSM303D internal sampling frequency. - * - * @param frequency The internal sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int set_samplerate(unsigned frequency); -}; - -/** - * Helper class implementing the mag driver node. - */ -class LSM303D_mag : public device::CDev -{ -public: - LSM303D_mag(LSM303D *parent); - ~LSM303D_mag(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - -protected: - friend class LSM303D; - - void parent_poll_notify(); -private: - LSM303D *_parent; - - void measure(); - - void measure_trampoline(void *arg); -}; - - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - - -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), - _mag(new LSM303D_mag(this)), - _call_accel_interval(0), - _call_mag_interval(0), - _num_accel_reports(0), - _next_accel_report(0), - _oldest_accel_report(0), - _accel_reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _num_mag_reports(0), - _next_mag_report(0), - _oldest_mag_report(0), - _mag_reports(nullptr), - _mag_range_scale(0.0f), - _mag_range_ga(0.0f), - _current_accel_rate(0), - _current_accel_range(0), - _current_mag_rate(0), - _current_mag_range(0), - _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) -{ - // enable debug() calls - _debug_enabled = true; - - /* XXX fix this default values */ - _accel_range_scale = 1.0f; - _mag_range_scale = 1.0f; - - // default scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - _mag_scale.x_offset = 0; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; - _mag_scale.z_scale = 1.0f; -} - -LSM303D::~LSM303D() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_accel_reports != nullptr) - delete[] _accel_reports; - if (_mag_reports != nullptr) - delete[] _mag_reports; - - delete _mag; - - /* delete the perf counter */ - perf_free(_accel_sample_perf); - perf_free(_mag_sample_perf); -} - -int -LSM303D::init() -{ - int ret = ERROR; - int mag_ret; - int fd_mag; - - /* do SPI init (and probe) first */ - if (SPI::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_accel_reports = 2; - _oldest_accel_report = _next_accel_report = 0; - _accel_reports = new struct accel_report[_num_accel_reports]; - - if (_accel_reports == nullptr) - goto out; - - /* advertise accel topic */ - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); - - _num_mag_reports = 2; - _oldest_mag_report = _next_mag_report = 0; - _mag_reports = new struct mag_report[_num_mag_reports]; - - if (_mag_reports == nullptr) - goto out; - - /* advertise mag topic */ - memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - - /* XXX do this with ioctls */ - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); - - /* XXX should we enable FIFO */ - - set_range(500); /* default to 500dps */ - set_samplerate(0); /* max sample rate */ - -// _current_accel_rate = 100; - - /* XXX test this when another mag is used */ - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; - } - - ret = OK; -out: - return ret; -} - -int -LSM303D::probe() -{ - /* read dummy value to void to clear SPI statemachine on sensor */ - (void)read_reg(ADDR_WHO_AM_I); - - /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) - return OK; - - return -EIO; -} - -ssize_t -LSM303D::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct accel_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_call_accel_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_accel_report != _next_accel_report) { - memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); - ret += sizeof(_accel_reports[0]); - INCREMENT(_oldest_accel_report, _num_accel_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _oldest_accel_report = _next_accel_report = 0; - measure(); - - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); - ret = sizeof(*_accel_reports); - - return ret; -} - -ssize_t -LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct mag_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_call_mag_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_mag_report != _next_mag_report) { - memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); - ret += sizeof(_mag_reports[0]); - INCREMENT(_oldest_mag_report, _num_mag_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _oldest_mag_report = _next_mag_report = 0; - measure(); - - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); - ret = sizeof(*_mag_reports); - - return ret; -} - -int -LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_accel_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_accel_interval; - - case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; - - case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -int -LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_mag_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* 50 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_mag_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = ticks; - - - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_mag_interval; - case SENSORIOCSQUEUEDEPTH: - case SENSORIOCGQUEUEDEPTH: - case SENSORIOCRESET: - return ioctl(filp, cmd, arg); - - case MAGIOCSSAMPLERATE: -// case MAGIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; - - case MAGIOCSLOWPASS: -// case MAGIOCGLOWPASS: - /* XXX not implemented */ -// _set_dlpf_filter((uint16_t)arg); - return -EINVAL; - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - case MAGIOCSRANGE: -// case MAGIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _mag_range_scale = xx - // _mag_range_ga = xx - return -EINVAL; - - case MAGIOCSELFTEST: - /* XXX not implemented */ -// return self_test(); - return -EINVAL; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -uint8_t -LSM303D::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -LSM303D::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int -LSM303D::set_range(unsigned max_dps) -{ - /* XXX implement this */ -// uint8_t bits = REG4_BDU; -// -// if (max_dps == 0) -// max_dps = 2000; -// -// if (max_dps <= 250) { -// _current_range = 250; -// bits |= RANGE_250DPS; -// -// } else if (max_dps <= 500) { -// _current_range = 500; -// bits |= RANGE_500DPS; -// -// } else if (max_dps <= 2000) { -// _current_range = 2000; -// bits |= RANGE_2000DPS; -// -// } else { -// return -EINVAL; -// } -// -// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; -// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; -// write_reg(ADDR_CTRL_REG4, bits); - - return OK; -} - -int -LSM303D::set_samplerate(unsigned frequency) -{ - /* XXX implement this */ -// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; -// -// if (frequency == 0) -// frequency = 760; -// -// if (frequency <= 95) { -// _current_rate = 95; -// bits |= RATE_95HZ_LP_25HZ; -// -// } else if (frequency <= 190) { -// _current_rate = 190; -// bits |= RATE_190HZ_LP_25HZ; -// -// } else if (frequency <= 380) { -// _current_rate = 380; -// bits |= RATE_380HZ_LP_30HZ; -// -// } else if (frequency <= 760) { -// _current_rate = 760; -// bits |= RATE_760HZ_LP_30HZ; -// -// } else { -// return -EINVAL; -// } -// -// write_reg(ADDR_CTRL_REG1, bits); - - return OK; -} - -void -LSM303D::start() -{ - /* make sure we are stopped first */ - stop(); - - /* reset the report ring */ - _oldest_accel_report = _next_accel_report = 0; - _oldest_mag_report = _next_mag_report = 0; - - /* start polling at the specified rate */ - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); -} - -void -LSM303D::stop() -{ - hrt_cancel(&_accel_call); - hrt_cancel(&_mag_call); -} - -void -LSM303D::measure_trampoline(void *arg) -{ - LSM303D *dev = (LSM303D *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -LSM303D::mag_measure_trampoline(void *arg) -{ - LSM303D *dev = (LSM303D *)arg; - - /* make another measurement */ - dev->mag_measure(); -} - -void -LSM303D::measure() -{ - /* status register and data as read back from the device */ - -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_accel_report; -#pragma pack(pop) - - accel_report *accel_report = &_accel_reports[_next_accel_report]; - - /* start the performance counter */ - perf_begin(_accel_sample_perf); - - /* fetch data from the sensor */ - raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); - - /* XXX adapt the comment to specs */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - - accel_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - accel_report->x_raw = raw_accel_report.x; - accel_report->y_raw = raw_accel_report.y; - accel_report->z_raw = raw_accel_report.z; - - accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_accel_report == _oldest_accel_report) - INCREMENT(_oldest_accel_report, _num_accel_reports); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); - - /* stop the perf counter */ - perf_end(_accel_sample_perf); -} - -void -LSM303D::mag_measure() -{ - /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_mag_report; -#pragma pack(pop) - - mag_report *mag_report = &_mag_reports[_next_mag_report]; - - /* start the performance counter */ - perf_begin(_mag_sample_perf); - - /* fetch data from the sensor */ - raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); - - /* XXX adapt the comment to specs */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - - mag_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - mag_report->x_raw = raw_mag_report.x; - mag_report->y_raw = raw_mag_report.y; - mag_report->z_raw = raw_mag_report.z; - mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_mag_report, _num_mag_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_mag_report == _oldest_mag_report) - INCREMENT(_oldest_mag_report, _num_mag_reports); - - /* XXX please check this poll_notify, is it the right one? */ - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); - - /* stop the perf counter */ - perf_end(_mag_sample_perf); -} - -void -LSM303D::print_info() -{ - perf_print_counter(_accel_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); - perf_print_counter(_mag_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); -} - -LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), - _parent(parent) -{ -} - -LSM303D_mag::~LSM303D_mag() -{ -} - -void -LSM303D_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -ssize_t -LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->mag_read(filp, buffer, buflen); -} - -int -LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - return _parent->mag_ioctl(filp, cmd, arg); -} - -void -LSM303D_mag::measure() -{ - _parent->mag_measure(); -} - -void -LSM303D_mag::measure_trampoline(void *arg) -{ - _parent->mag_measure_trampoline(arg); -} - -/** - * Local functions in support of the shell command. - */ -namespace lsm303d -{ - -LSM303D *g_dev; - -void start(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd, fd_mag; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); - - /* don't fail if open cannot be opened */ - if (0 <= fd_mag) { - if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - } - - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int fd_accel = -1; - struct accel_report a_report; - ssize_t sz; - - /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd_accel, &a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) - err(1, "immediate read failed"); - - /* XXX fix the test output */ -// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); -// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); -// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); - warnx("accel x: \t%d\traw", (int)a_report.x_raw); - warnx("accel y: \t%d\traw", (int)a_report.y_raw); - warnx("accel z: \t%d\traw", (int)a_report.z_raw); -// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); - - - - int fd_mag = -1; - struct mag_report m_report; - - /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); - - if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd_mag, &m_report, sizeof(m_report)); - - if (sz != sizeof(m_report)) - err(1, "immediate read failed"); - - /* XXX fix the test output */ - warnx("mag x: \t%d\traw", (int)m_report.x_raw); - warnx("mag y: \t%d\traw", (int)m_report.y_raw); - warnx("mag z: \t%d\traw", (int)m_report.z_raw); - - /* XXX add poll-rate tests here too */ - -// reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running\n"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - - -} // namespace - -int -lsm303d_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - - */ - if (!strcmp(argv[1], "start")) - lsm303d::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - lsm303d::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - lsm303d::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - lsm303d::info(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/src/device/lsm303d/module.mk b/src/device/lsm303d/module.mk deleted file mode 100644 index e93b1e331..000000000 --- a/src/device/lsm303d/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# LSM303D accel/mag driver -# - -MODULE_COMMAND = lsm303d -SRCS = lsm303d.cpp diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp deleted file mode 100644 index d3865f053..000000000 --- a/src/device/px4fmu/fmu.cpp +++ /dev/null @@ -1,1217 +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 fmu.cpp - * - * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#if defined(CONFIG_ARCH_BOARD_PX4FMU) -# include -# define FMU_HAVE_PPM -#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) -# include -# undef FMU_HAVE_PPM -#else -# error Unrecognised FMU board. -#endif - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#ifdef FMU_HAVE_PPM -# include -#endif - -class PX4FMU : public device::CDev -{ -public: - enum Mode { - MODE_NONE, - MODE_2PWM, - MODE_4PWM, - MODE_6PWM, - }; - PX4FMU(); - virtual ~PX4FMU(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual ssize_t write(file *filp, const char *buffer, size_t len); - - virtual int init(); - - int set_mode(Mode mode); - - int set_pwm_alt_rate(unsigned rate); - int set_pwm_alt_channels(uint32_t channels); - -private: - static const unsigned _max_actuators = 4; - - Mode _mode; - unsigned _pwm_default_rate; - unsigned _pwm_alt_rate; - uint32_t _pwm_alt_rate_channels; - unsigned _current_update_rate; - int _task; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; - unsigned _num_outputs; - bool _primary_pwm_device; - - volatile bool _task_should_exit; - bool _armed; - - MixerGroup *_mixers; - - actuator_controls_s _controls; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main() __attribute__((noreturn)); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); - -}; - -const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {0, GPIO_VDD_5V_PERIPH_EN, 0}, - {GPIO_5V_HIPOWER_OC, 0, 0}, - {GPIO_5V_PERIPH_OC, 0, 0}, -#endif -}; - -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); - -namespace -{ - -PX4FMU *g_fmu; - -} // namespace - -PX4FMU::PX4FMU() : - CDev("fmuservo", "/dev/px4fmu"), - _mode(MODE_NONE), - _pwm_default_rate(50), - _pwm_alt_rate(50), - _pwm_alt_rate_channels(0), - _current_update_rate(0), - _task(-1), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), - _t_actuators_effective(0), - _num_outputs(0), - _primary_pwm_device(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr) -{ - _debug_enabled = true; -} - -PX4FMU::~PX4FMU() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); - } - - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - g_fmu = nullptr; -} - -int -PX4FMU::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) - return ret; - - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } - - /* reset GPIOs */ - gpio_reset(); - - /* start the IO interface task */ - _task = task_spawn("fmuservo", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&PX4FMU::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -void -PX4FMU::task_main_trampoline(int argc, char *argv[]) -{ - g_fmu->task_main(); -} - -int -PX4FMU::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: // v1 multi-port with flow control lines as PWM - debug("MODE_2PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); - - break; - - case MODE_4PWM: // v1 multi-port as 4 PWM outs - debug("MODE_4PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xf); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); - - break; - - case MODE_6PWM: // v2 PWMs as 6 PWM outs - debug("MODE_6PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3f); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); - - break; - - case MODE_NONE: - debug("MODE_NONE"); - - _pwm_default_rate = 10; /* artificially reduced output rate */ - _pwm_alt_rate = 10; - _pwm_alt_rate_channels = 0; - - /* disable servo outputs - no need to set rates */ - up_pwm_servo_deinit(); - - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - -int -PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) -{ - debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); - - for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < _max_actuators; group++) { - - // get the channel mask for this rate group - uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) - continue; - - // all channels in the group must be either default or alt-rate - uint32_t alt = rate_map & mask; - - if (pass == 0) { - // preflight - if ((alt != 0) && (alt != mask)) { - warn("rate group %u mask %x bad overlap %x", group, mask, alt); - // not a legal map, bail - return -EINVAL; - } - } else { - // set it - errors here are unexpected - if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { - warn("rate group set alt failed"); - return -EINVAL; - } - } else { - if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { - warn("rate group set default failed"); - return -EINVAL; - } - } - } - } - } - _pwm_alt_rate_channels = rate_map; - _pwm_default_rate = default_rate; - _pwm_alt_rate = alt_rate; - - return OK; -} - -int -PX4FMU::set_pwm_alt_rate(unsigned rate) -{ - return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); -} - -int -PX4FMU::set_pwm_alt_channels(uint32_t channels) -{ - return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); -} - -void -PX4FMU::task_main() -{ - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - /* force a reset of the update rate */ - _current_update_rate = 0; - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - -#ifdef FMU_HAVE_PPM - // rc input, published to ORB - struct rc_input_values rc_in; - orb_advert_t to_input_rc = 0; - - memset(&rc_in, 0, sizeof(rc_in)); - rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; -#endif - - log("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - /* - * Adjust actuator topic update rate to keep up with - * the highest servo update rate configured. - * - * We always mix at max rate; some channels may update slower. - */ - unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - if (_current_update_rate != max_rate) { - _current_update_rate = max_rate; - int update_rate_in_ms = int(1000 / _current_update_rate); - - /* reject faster than 500 Hz updates */ - if (update_rate_in_ms < 2) { - update_rate_in_ms = 2; - } - /* reject slower than 10 Hz updates */ - if (update_rate_in_ms > 100) { - update_rate_in_ms = 100; - } - - debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); - - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; - } - - /* sleep waiting for data, stopping to check for PPM - * input at 100Hz */ - int ret = ::poll(&fds[0], 2, 10); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - usleep(1000000); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - unsigned num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - case MODE_4PWM: - num_outputs = 4; - break; - case MODE_6PWM: - num_outputs = 6; - break; - default: - num_outputs = 0; - break; - } - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } - - /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); - } - - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - - /* update PWM servo armed status if armed and not locked down */ - up_pwm_servo_arm(aa.armed && !aa.lockdown); - } - -#ifdef FMU_HAVE_PPM - // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { - // we have a new PPM frame. Publish it. - rc_in.channel_count = ppm_decoded_channels; - if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { - rc_in.channel_count = RC_INPUT_MAX_CHANNELS; - } - for (uint8_t i=0; icontrol[control_index]; - return 0; -} - -int -PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret; - - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - - /* try it as a GPIO ioctl first */ - ret = gpio_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) - return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch (_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_6PWM: - ret = pwm_ioctl(filp, cmd, arg); - break; - - default: - debug("not in a PWM mode"); - break; - } - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) - ret = CDev::ioctl(filp, cmd, arg); - - return ret; -} - -int -PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); - break; - - case PWM_SERVO_SELECT_UPDATE_RATE: - ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); - break; - - case PWM_SERVO_SET(5): - case PWM_SERVO_SET(4): - if (_mode < MODE_6PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(3): - case PWM_SERVO_SET(2): - if (_mode < MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(1): - case PWM_SERVO_SET(0): - if (arg < 2100) { - up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(5): - case PWM_SERVO_GET(4): - if (_mode < MODE_6PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(3): - case PWM_SERVO_GET(2): - if (_mode < MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(1): - case PWM_SERVO_GET(0): - *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); - break; - - case PWM_SERVO_GET_RATEGROUP(0): - case PWM_SERVO_GET_RATEGROUP(1): - case PWM_SERVO_GET_RATEGROUP(2): - case PWM_SERVO_GET_RATEGROUP(3): - case PWM_SERVO_GET_RATEGROUP(4): - case PWM_SERVO_GET_RATEGROUP(5): - *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); - break; - - case PWM_SERVO_GET_COUNT: - case MIXERIOCGETOUTPUTCOUNT: - switch (_mode) { - case MODE_6PWM: - *(unsigned *)arg = 6; - break; - case MODE_4PWM: - *(unsigned *)arg = 4; - break; - case MODE_2PWM: - *(unsigned *)arg = 2; - break; - default: - ret = -EINVAL; - break; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - _mixers->add_mixer(mixer); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -/* - this implements PWM output via a write() method, for compatibility - with px4io - */ -ssize_t -PX4FMU::write(file *filp, const char *buffer, size_t len) -{ - unsigned count = len / 2; - uint16_t values[6]; - - if (count > 6) { - // we have at most 6 outputs - count = 6; - } - - // allow for misaligned values - memcpy(values, buffer, count * 2); - - for (uint8_t i = 0; i < count; i++) { - up_pwm_servo_set(i, values[i]); - } - return count * 2; -} - -void -PX4FMU::gpio_reset(void) -{ - /* - * Setup default GPIO config - all pins as GPIOs, input if - * possible otherwise output if possible. - */ - for (unsigned i = 0; i < _ngpio; i++) { - if (_gpio_tab[i].input != 0) { - stm32_configgpio(_gpio_tab[i].input); - } else if (_gpio_tab[i].output != 0) { - stm32_configgpio(_gpio_tab[i].output); - } - } - -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - /* if we have a GPIO direction control, set it to zero (input) */ - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - stm32_configgpio(GPIO_GPIO_DIR); -#endif -} - -void -PX4FMU::gpio_set_function(uint32_t gpios, int function) -{ -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - /* - * GPIOs 0 and 1 must have the same direction as they are buffered - * by a shared 2-port driver. Any attempt to set either sets both. - */ - if (gpios & 3) { - gpios |= 3; - - /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) - stm32_gpiowrite(GPIO_GPIO_DIR, 1); - } -#endif - - /* configure selected GPIOs as required */ - for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1 << i)) { - switch (function) { - case GPIO_SET_INPUT: - stm32_configgpio(_gpio_tab[i].input); - break; - - case GPIO_SET_OUTPUT: - stm32_configgpio(_gpio_tab[i].output); - break; - - case GPIO_SET_ALT_1: - if (_gpio_tab[i].alt != 0) - stm32_configgpio(_gpio_tab[i].alt); - - break; - } - } - } - -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - /* flip buffer to input mode if required */ - if ((GPIO_SET_INPUT == function) && (gpios & 3)) - stm32_gpiowrite(GPIO_GPIO_DIR, 0); -#endif -} - -void -PX4FMU::gpio_write(uint32_t gpios, int function) -{ - int value = (function == GPIO_SET) ? 1 : 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1 << i)) - stm32_gpiowrite(_gpio_tab[i].output, value); -} - -uint32_t -PX4FMU::gpio_read(void) -{ - uint32_t bits = 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) - bits |= (1 << i); - - return bits; -} - -int -PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - - case GPIO_RESET: - gpio_reset(); - break; - - case GPIO_SET_OUTPUT: - case GPIO_SET_INPUT: - case GPIO_SET_ALT_1: - gpio_set_function(arg, cmd); - break; - - case GPIO_SET_ALT_2: - case GPIO_SET_ALT_3: - case GPIO_SET_ALT_4: - ret = -EINVAL; - break; - - case GPIO_SET: - case GPIO_CLEAR: - gpio_write(arg, cmd); - break; - - case GPIO_GET: - *(uint32_t *)arg = gpio_read(); - break; - - default: - ret = -ENOTTY; - } - - unlock(); - - return ret; -} - -namespace -{ - -enum PortMode { - PORT_MODE_UNSET = 0, - PORT_FULL_GPIO, - PORT_FULL_SERIAL, - PORT_FULL_PWM, - PORT_GPIO_AND_SERIAL, - PORT_PWM_AND_SERIAL, - PORT_PWM_AND_GPIO, -}; - -PortMode g_port_mode; - -int -fmu_new_mode(PortMode new_mode) -{ - uint32_t gpio_bits; - PX4FMU::Mode servo_mode; - - /* reset to all-inputs */ - g_fmu->ioctl(0, GPIO_RESET, 0); - - gpio_bits = 0; - servo_mode = PX4FMU::MODE_NONE; - - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT_FULL_PWM: -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - /* select 4-pin PWM mode */ - servo_mode = PX4FMU::MODE_4PWM; -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) - servo_mode = PX4FMU::MODE_6PWM; -#endif - break; - - /* mixed modes supported on v1 board only */ -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_GPIO_AND_SERIAL: - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = PX4FMU::MODE_2PWM; - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = PX4FMU::MODE_2PWM; - break; -#endif - default: - return -1; - } - - /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) - g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - - /* (re)set the PWM output mode */ - g_fmu->set_mode(servo_mode); - - return OK; -} - -int -fmu_start(void) -{ - int ret = OK; - - if (g_fmu == nullptr) { - - g_fmu = new PX4FMU; - - if (g_fmu == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_fmu->init(); - - if (ret != OK) { - delete g_fmu; - g_fmu = nullptr; - } - } - } - - return ret; -} - -void -test(void) -{ - int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - - if (fd < 0) - errx(1, "open fail"); - - if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); - -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) - if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); -#endif - - close(fd); - - exit(0); -} - -void -fake(int argc, char *argv[]) -{ - if (argc < 5) - errx(1, "fmu fake (values -100 .. 100)"); - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle < 0) - errx(1, "advertise failed"); - - actuator_armed_s aa; - - aa.armed = true; - aa.lockdown = false; - - handle = orb_advertise(ORB_ID(actuator_armed), &aa); - - if (handle < 0) - errx(1, "advertise failed 2"); - - exit(0); -} - -} // namespace - -extern "C" __EXPORT int fmu_main(int argc, char *argv[]); - -int -fmu_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNSET; - const char *verb = argv[1]; - - if (fmu_start() != OK) - errx(1, "failed to start the FMU driver"); - - /* - * Mode switches. - */ - if (!strcmp(verb, "mode_gpio")) { - new_mode = PORT_FULL_GPIO; - - } else if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT_FULL_PWM; - -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - } else if (!strcmp(verb, "mode_serial")) { - new_mode = PORT_FULL_SERIAL; - - } else if (!strcmp(verb, "mode_gpio_serial")) { - new_mode = PORT_GPIO_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_serial")) { - new_mode = PORT_PWM_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_gpio")) { - new_mode = PORT_PWM_AND_GPIO; -#endif - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNSET) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) - return OK; - - /* switch modes */ - int ret = fmu_new_mode(new_mode); - exit(ret == OK ? 0 : 1); - } - - if (!strcmp(verb, "test")) - test(); - - if (!strcmp(verb, "fake")) - fake(argc - 1, argv + 1); - - fprintf(stderr, "FMU: unrecognised command, try:\n"); -#if defined(CONFIG_ARCH_BOARD_PX4FMU) - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) - fprintf(stderr, " mode_gpio, mode_pwm\n"); -#endif - exit(1); -} diff --git a/src/device/px4fmu/module.mk b/src/device/px4fmu/module.mk deleted file mode 100644 index 05bc7a5b3..000000000 --- a/src/device/px4fmu/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# Interface driver for the PX4FMU board -# - -MODULE_COMMAND = fmu -SRCS = fmu.cpp diff --git a/src/device/rgbled/module.mk b/src/device/rgbled/module.mk deleted file mode 100644 index 39b53ec9e..000000000 --- a/src/device/rgbled/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# TCA62724FMG driver for RGB LED -# - -MODULE_COMMAND = rgbled -SRCS = rgbled.cpp diff --git a/src/device/rgbled/rgbled.cpp b/src/device/rgbled/rgbled.cpp deleted file mode 100644 index dc1e469c0..000000000 --- a/src/device/rgbled/rgbled.cpp +++ /dev/null @@ -1,497 +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 rgbled.cpp - * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. - * - * - */ - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include "device/rgbled.h" - -#define LED_ONTIME 120 -#define LED_OFFTIME 120 - -#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ -#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ -#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ -#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ -#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ -#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ - -#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ -#define SETTING_ENABLE 0x02 /**< on */ - - -enum ledModes { - LED_MODE_TEST, - LED_MODE_SYSTEMSTATE, - LED_MODE_OFF -}; - -class RGBLED : public device::I2C -{ -public: - RGBLED(int bus, int rgbled); - virtual ~RGBLED(); - - - virtual int init(); - virtual int probe(); - virtual int info(); - virtual int setMode(enum ledModes mode); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - -private: - - enum ledColors { - LED_COLOR_OFF, - LED_COLOR_RED, - LED_COLOR_YELLOW, - LED_COLOR_PURPLE, - LED_COLOR_GREEN, - LED_COLOR_BLUE, - LED_COLOR_WHITE, - LED_COLOR_AMBER, - }; - - enum ledBlinkModes { - LED_BLINK_ON, - LED_BLINK_OFF - }; - - work_s _work; - - int led_colors[8]; - int led_blink; - - int mode; - int running; - - void setLEDColor(int ledcolor); - static void led_trampoline(void *arg); - void led(); - - int set(bool on, uint8_t r, uint8_t g, uint8_t b); - int set_on(bool on); - int set_rgb(uint8_t r, uint8_t g, uint8_t b); - int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); -}; - -/* for now, we only support one RGBLED */ -namespace -{ - RGBLED *g_rgbled; -} - - -extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); - -RGBLED::RGBLED(int bus, int rgbled) : - I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - led_colors({0,0,0,0,0,0,0,0}), - led_blink(LED_BLINK_OFF), - mode(LED_MODE_OFF), - running(false) -{ - memset(&_work, 0, sizeof(_work)); -} - -RGBLED::~RGBLED() -{ -} - -int -RGBLED::init() -{ - int ret; - ret = I2C::init(); - - if (ret != OK) { - warnx("I2C init failed"); - return ret; - } - - /* start off */ - set(false, 0, 0, 0); - - return OK; -} - -int -RGBLED::setMode(enum ledModes new_mode) -{ - switch (new_mode) { - case LED_MODE_SYSTEMSTATE: - case LED_MODE_TEST: - mode = new_mode; - if (!running) { - running = true; - set_on(true); - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); - } - break; - case LED_MODE_OFF: - - default: - if (running) { - running = false; - set_on(false); - } - mode = LED_MODE_OFF; - break; - } - - return OK; -} - -int -RGBLED::probe() -{ - int ret; - bool on, not_powersave; - uint8_t r, g, b; - - ret = get(on, not_powersave, r, g, b); - - return ret; -} - -int -RGBLED::info() -{ - int ret; - bool on, not_powersave; - uint8_t r, g, b; - - ret = get(on, not_powersave, r, g, b); - - if (ret == OK) { - /* we don't care about power-save mode */ - log("state: %s", on ? "ON" : "OFF"); - log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); - } else { - warnx("failed to read led"); - } - - return ret; -} - -int -RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = ENOTTY; - - switch (cmd) { - - default: - break; - } - - return ret; -} - - -void -RGBLED::led_trampoline(void *arg) -{ - RGBLED *rgbl = (RGBLED *)arg; - - rgbl->led(); -} - - - -void -RGBLED::led() -{ - static int led_thread_runcount=0; - static int led_interval = 1000; - - switch (mode) { - case LED_MODE_TEST: - /* Demo LED pattern for now */ - led_colors[0] = LED_COLOR_YELLOW; - led_colors[1] = LED_COLOR_AMBER; - led_colors[2] = LED_COLOR_RED; - led_colors[3] = LED_COLOR_PURPLE; - led_colors[4] = LED_COLOR_BLUE; - led_colors[5] = LED_COLOR_GREEN; - led_colors[6] = LED_COLOR_WHITE; - led_colors[7] = LED_COLOR_OFF; - led_blink = LED_BLINK_ON; - break; - - case LED_MODE_SYSTEMSTATE: - /* XXX TODO set pattern */ - led_colors[0] = LED_COLOR_OFF; - led_colors[1] = LED_COLOR_OFF; - led_colors[2] = LED_COLOR_OFF; - led_colors[3] = LED_COLOR_OFF; - led_colors[4] = LED_COLOR_OFF; - led_colors[5] = LED_COLOR_OFF; - led_colors[6] = LED_COLOR_OFF; - led_colors[7] = LED_COLOR_OFF; - led_blink = LED_BLINK_OFF; - - break; - - case LED_MODE_OFF: - default: - return; - break; - } - - - if (led_thread_runcount & 1) { - if (led_blink == LED_BLINK_ON) - setLEDColor(LED_COLOR_OFF); - led_interval = LED_OFFTIME; - } else { - setLEDColor(led_colors[(led_thread_runcount/2) % 8]); - led_interval = LED_ONTIME; - } - - led_thread_runcount++; - - - if(running) { - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); - } else { - set_on(false); - } -} - -void RGBLED::setLEDColor(int ledcolor) { - switch (ledcolor) { - case LED_COLOR_OFF: // off - set_rgb(0,0,0); - break; - case LED_COLOR_RED: // red - set_rgb(255,0,0); - break; - case LED_COLOR_YELLOW: // yellow - set_rgb(255,70,0); - break; - case LED_COLOR_PURPLE: // purple - set_rgb(255,0,255); - break; - case LED_COLOR_GREEN: // green - set_rgb(0,255,0); - break; - case LED_COLOR_BLUE: // blue - set_rgb(0,0,255); - break; - case LED_COLOR_WHITE: // white - set_rgb(255,255,255); - break; - case LED_COLOR_AMBER: // amber - set_rgb(255,20,0); - break; - } -} - -int -RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) -{ - uint8_t settings_byte = 0; - - if (on) - settings_byte |= SETTING_ENABLE; -/* powersave not used */ -// if (not_powersave) - settings_byte |= SETTING_NOT_POWERSAVE; - - const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte}; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -RGBLED::set_on(bool on) -{ - uint8_t settings_byte = 0; - - if (on) - settings_byte |= SETTING_ENABLE; - -/* powersave not used */ -// if (not_powersave) - settings_byte |= SETTING_NOT_POWERSAVE; - - const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - - -int -RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) -{ - uint8_t result[2]; - int ret; - - ret = transfer(nullptr, 0, &result[0], 2); - - if (ret == OK) { - on = result[0] & SETTING_ENABLE; - not_powersave = result[0] & SETTING_NOT_POWERSAVE; - /* XXX check, looks wrong */ - r = (result[0] & 0x0f)*255/15; - g = (result[1] & 0xf0)*255/15; - b = (result[1] & 0x0f)*255/15; - } - - return ret; -} - -void rgbled_usage(); - - -void rgbled_usage() { - warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); - warnx("options:"); - warnx("\t-b --bus i2cbus (3)"); - warnx("\t-a --ddr addr (9)"); -} - -int -rgbled_main(int argc, char *argv[]) -{ - - int i2cdevice = PX4_I2C_BUS_LED; - int rgbledadr = ADDR; /* 7bit */ - - int x; - - for (x = 1; x < argc; x++) { - if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { - if (argc > x + 1) { - i2cdevice = atoi(argv[x + 1]); - } - } - - if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) { - if (argc > x + 1) { - rgbledadr = atoi(argv[x + 1]); - } - } - - } - - if (!strcmp(argv[1], "start")) { - if (g_rgbled != nullptr) - errx(1, "already started"); - - g_rgbled = new RGBLED(i2cdevice, rgbledadr); - - if (g_rgbled == nullptr) - errx(1, "new failed"); - - if (OK != g_rgbled->init()) { - delete g_rgbled; - g_rgbled = nullptr; - errx(1, "init failed"); - } - - exit(0); - } - - - if (g_rgbled == nullptr) { - fprintf(stderr, "not started\n"); - rgbled_usage(); - exit(0); - } - - if (!strcmp(argv[1], "test")) { - g_rgbled->setMode(LED_MODE_TEST); - exit(0); - } - - if (!strcmp(argv[1], "systemstate")) { - g_rgbled->setMode(LED_MODE_SYSTEMSTATE); - exit(0); - } - - if (!strcmp(argv[1], "info")) { - g_rgbled->info(); - exit(0); - } - - if (!strcmp(argv[1], "off")) { - g_rgbled->setMode(LED_MODE_OFF); - exit(0); - } - - - /* things that require access to the device */ - int fd = open(RGBLED_DEVICE_PATH, 0); - if (fd < 0) - err(1, "can't open RGBLED device"); - - rgbled_usage(); - exit(0); -} diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk new file mode 100644 index 000000000..eb8841e4d --- /dev/null +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -0,0 +1,9 @@ +# +# Board-specific startup code for the PX4FMUv2 +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c diff --git a/src/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmuv2/px4fmu_can.c new file mode 100644 index 000000000..86ba183b8 --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "px4fmu_internal.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c new file mode 100644 index 000000000..1d99f15bf --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + stm32_configgpio(GPIO_ADC1_IN12); + + /* configure power supply control pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + // initial LED state + // XXX need to make this work again +// drv_led_start(); + up_ledoff(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + /* Get the SPI port for the FRAM */ + + message("[boot] Initializing SPI port 2\n"); + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 2\n"); + + /* XXX need a driver to bind the FRAM to */ + + //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); + + return OK; +} diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h new file mode 100644 index 000000000..cc4e9529d --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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 px4fmu_internal.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) + +/* External interrupts */ + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c new file mode 100644 index 000000000..14e4052be --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c new file mode 100644 index 000000000..f90f25071 --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu_spi.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c new file mode 100644 index 000000000..b0b669fbe --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp new file mode 100644 index 000000000..32030a1f7 --- /dev/null +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -0,0 +1,1290 @@ +/**************************************************************************** + * + * 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 lsm303d.cpp + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + + + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_L_T 0x05 +#define ADDR_OUT_H_T 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_OUT_TEMP_A 0x26 +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG0 0x1F +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG7 0x26 + +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) + +#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) + +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) + +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) + +#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) +#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) +#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) +#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) + +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) + + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + + +class LSM303D_mag; + +class LSM303D : public device::SPI +{ +public: + LSM303D(int bus, const char* path, spi_dev_e device); + virtual ~LSM303D(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + + friend class LSM303D_mag; + + virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + LSM303D_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_accel_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + + unsigned _num_mag_reports; + volatile unsigned _next_mag_report; + volatile unsigned _oldest_mag_report; + struct mag_report *_mag_reports; + + struct mag_scale _mag_scale; + float _mag_range_scale; + float _mag_range_ga; + orb_advert_t _mag_topic; + + unsigned _current_accel_rate; + unsigned _current_accel_range; + + unsigned _current_mag_rate; + unsigned _current_mag_range; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ + static void mag_measure_trampoline(void *arg); + + /** + * Fetch accel measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Fetch mag measurements from the sensor and update the report ring. + */ + void mag_measure(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D measurement range. + * + * @param max_dps The measurement range is set to permit reading at least + * this rate in degrees per second. + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_dps); + + /** + * Set the LSM303D internal sampling frequency. + * + * @param frequency The internal sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); +}; + +/** + * Helper class implementing the mag driver node. + */ +class LSM303D_mag : public device::CDev +{ +public: + LSM303D_mag(LSM303D *parent); + ~LSM303D_mag(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class LSM303D; + + void parent_poll_notify(); +private: + LSM303D *_parent; + + void measure(); + + void measure_trampoline(void *arg); +}; + + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + _mag(new LSM303D_mag(this)), + _call_accel_interval(0), + _call_mag_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), + _num_mag_reports(0), + _next_mag_report(0), + _oldest_mag_report(0), + _mag_reports(nullptr), + _mag_range_scale(0.0f), + _mag_range_ga(0.0f), + _current_accel_rate(0), + _current_accel_range(0), + _current_mag_rate(0), + _current_mag_range(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) +{ + // enable debug() calls + _debug_enabled = true; + + /* XXX fix this default values */ + _accel_range_scale = 1.0f; + _mag_range_scale = 1.0f; + + // default scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0; + _mag_scale.z_scale = 1.0f; +} + +LSM303D::~LSM303D() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete[] _accel_reports; + if (_mag_reports != nullptr) + delete[] _mag_reports; + + delete _mag; + + /* delete the perf counter */ + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); +} + +int +LSM303D::init() +{ + int ret = ERROR; + int mag_ret; + int fd_mag; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; + + if (_accel_reports == nullptr) + goto out; + + /* advertise accel topic */ + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + + _num_mag_reports = 2; + _oldest_mag_report = _next_mag_report = 0; + _mag_reports = new struct mag_report[_num_mag_reports]; + + if (_mag_reports == nullptr) + goto out; + + /* advertise mag topic */ + memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + + /* XXX do this with ioctls */ + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); + + /* XXX should we enable FIFO */ + + set_range(500); /* default to 500dps */ + set_samplerate(0); /* max sample rate */ + +// _current_accel_rate = 100; + + /* XXX test this when another mag is used */ + /* do CDev init for the mag device node, keep it optional */ + mag_ret = _mag->init(); + + if (mag_ret != OK) { + _mag_topic = -1; + } + + ret = OK; +out: + return ret; +} + +int +LSM303D::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +LSM303D::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_accel_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_accel_report = _next_accel_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); + + return ret; +} + +ssize_t +LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_mag_report != _next_mag_report) { + memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); + ret += sizeof(_mag_reports[0]); + INCREMENT(_oldest_mag_report, _num_mag_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_mag_report = _next_mag_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); + ret = sizeof(*_mag_reports); + + return ret; +} + +int +LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_accel_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_accel_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_accel_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_accel_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 50 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + case SENSORIOCSQUEUEDEPTH: + case SENSORIOCGQUEUEDEPTH: + case SENSORIOCRESET: + return ioctl(filp, cmd, arg); + + case MAGIOCSSAMPLERATE: +// case MAGIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case MAGIOCSLOWPASS: +// case MAGIOCGLOWPASS: + /* XXX not implemented */ +// _set_dlpf_filter((uint16_t)arg); + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: +// case MAGIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _mag_range_scale = xx + // _mag_range_ga = xx + return -EINVAL; + + case MAGIOCSELFTEST: + /* XXX not implemented */ +// return self_test(); + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +LSM303D::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +LSM303D::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +LSM303D::set_range(unsigned max_dps) +{ + /* XXX implement this */ +// uint8_t bits = REG4_BDU; +// +// if (max_dps == 0) +// max_dps = 2000; +// +// if (max_dps <= 250) { +// _current_range = 250; +// bits |= RANGE_250DPS; +// +// } else if (max_dps <= 500) { +// _current_range = 500; +// bits |= RANGE_500DPS; +// +// } else if (max_dps <= 2000) { +// _current_range = 2000; +// bits |= RANGE_2000DPS; +// +// } else { +// return -EINVAL; +// } +// +// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; +// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; +// write_reg(ADDR_CTRL_REG4, bits); + + return OK; +} + +int +LSM303D::set_samplerate(unsigned frequency) +{ + /* XXX implement this */ +// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; +// +// if (frequency == 0) +// frequency = 760; +// +// if (frequency <= 95) { +// _current_rate = 95; +// bits |= RATE_95HZ_LP_25HZ; +// +// } else if (frequency <= 190) { +// _current_rate = 190; +// bits |= RATE_190HZ_LP_25HZ; +// +// } else if (frequency <= 380) { +// _current_rate = 380; +// bits |= RATE_380HZ_LP_30HZ; +// +// } else if (frequency <= 760) { +// _current_rate = 760; +// bits |= RATE_760HZ_LP_30HZ; +// +// } else { +// return -EINVAL; +// } +// +// write_reg(ADDR_CTRL_REG1, bits); + + return OK; +} + +void +LSM303D::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_accel_report = _next_accel_report = 0; + _oldest_mag_report = _next_mag_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); +} + +void +LSM303D::stop() +{ + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); +} + +void +LSM303D::measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +LSM303D::mag_measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + +void +LSM303D::measure() +{ + /* status register and data as read back from the device */ + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; +#pragma pack(pop) + + accel_report *accel_report = &_accel_reports[_next_accel_report]; + + /* start the performance counter */ + perf_begin(_accel_sample_perf); + + /* fetch data from the sensor */ + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + /* XXX adapt the comment to specs */ + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + accel_report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + accel_report->x_raw = raw_accel_report.x; + accel_report->y_raw = raw_accel_report.y; + accel_report->z_raw = raw_accel_report.z; + + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_accel_report, _num_accel_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +LSM303D::mag_measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report *mag_report = &_mag_reports[_next_mag_report]; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* XXX adapt the comment to specs */ + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + mag_report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + mag_report->x_raw = raw_mag_report.x; + mag_report->y_raw = raw_mag_report.y; + mag_report->z_raw = raw_mag_report.z; + mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; +// report->scaling = _gyro_range_scale; +// report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_mag_report, _num_mag_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_mag_report == _oldest_mag_report) + INCREMENT(_oldest_mag_report, _num_mag_reports); + + /* XXX please check this poll_notify, is it the right one? */ + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + + /* stop the perf counter */ + perf_end(_mag_sample_perf); +} + +void +LSM303D::print_info() +{ + perf_print_counter(_accel_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); + perf_print_counter(_mag_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); +} + +LSM303D_mag::LSM303D_mag(LSM303D *parent) : + CDev("LSM303D_mag", MAG_DEVICE_PATH), + _parent(parent) +{ +} + +LSM303D_mag::~LSM303D_mag() +{ +} + +void +LSM303D_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->mag_ioctl(filp, cmd, arg); +} + +void +LSM303D_mag::measure() +{ + _parent->mag_measure(); +} + +void +LSM303D_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd, fd_mag; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + } + + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_accel = -1; + struct accel_report a_report; + ssize_t sz; + + /* get the driver */ + fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd_accel < 0) + err(1, "%s open failed", ACCEL_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_accel, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) + err(1, "immediate read failed"); + + /* XXX fix the test output */ +// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); +// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); +// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); + warnx("accel x: \t%d\traw", (int)a_report.x_raw); + warnx("accel y: \t%d\traw", (int)a_report.y_raw); + warnx("accel z: \t%d\traw", (int)a_report.z_raw); +// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + + + + int fd_mag = -1; + struct mag_report m_report; + + /* get the driver */ + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) + err(1, "%s open failed", MAG_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_mag, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) + err(1, "immediate read failed"); + + /* XXX fix the test output */ + warnx("mag x: \t%d\traw", (int)m_report.x_raw); + warnx("mag y: \t%d\traw", (int)m_report.y_raw); + warnx("mag z: \t%d\traw", (int)m_report.z_raw); + + /* XXX add poll-rate tests here too */ + +// reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + lsm303d::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + lsm303d::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + lsm303d::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + lsm303d::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk new file mode 100644 index 000000000..e93b1e331 --- /dev/null +++ b/src/drivers/lsm303d/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = lsm303d +SRCS = lsm303d.cpp diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp new file mode 100644 index 000000000..d3865f053 --- /dev/null +++ b/src/drivers/px4fmu/fmu.cpp @@ -0,0 +1,1217 @@ +/**************************************************************************** + * + * 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 fmu.cpp + * + * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#if defined(CONFIG_ARCH_BOARD_PX4FMU) +# include +# define FMU_HAVE_PPM +#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +# include +# undef FMU_HAVE_PPM +#else +# error Unrecognised FMU board. +#endif + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#ifdef FMU_HAVE_PPM +# include +#endif + +class PX4FMU : public device::CDev +{ +public: + enum Mode { + MODE_NONE, + MODE_2PWM, + MODE_4PWM, + MODE_6PWM, + }; + PX4FMU(); + virtual ~PX4FMU(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + + virtual int init(); + + int set_mode(Mode mode); + + int set_pwm_alt_rate(unsigned rate); + int set_pwm_alt_channels(uint32_t channels); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + unsigned _pwm_default_rate; + unsigned _pwm_alt_rate; + uint32_t _pwm_alt_rate_channels; + unsigned _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + +}; + +const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {GPIO_5V_HIPOWER_OC, 0, 0}, + {GPIO_5V_PERIPH_OC, 0, 0}, +#endif +}; + +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); + +namespace +{ + +PX4FMU *g_fmu; + +} // namespace + +PX4FMU::PX4FMU() : + CDev("fmuservo", "/dev/px4fmu"), + _mode(MODE_NONE), + _pwm_default_rate(50), + _pwm_alt_rate(50), + _pwm_alt_rate_channels(0), + _current_update_rate(0), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _t_actuators_effective(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +PX4FMU::~PX4FMU() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_fmu = nullptr; +} + +int +PX4FMU::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + gpio_reset(); + + /* start the IO interface task */ + _task = task_spawn("fmuservo", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&PX4FMU::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +PX4FMU::task_main_trampoline(int argc, char *argv[]) +{ + g_fmu->task_main(); +} + +int +PX4FMU::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: // v1 multi-port with flow control lines as PWM + debug("MODE_2PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_4PWM: // v1 multi-port as 4 PWM outs + debug("MODE_4PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_6PWM: // v2 PWMs as 6 PWM outs + debug("MODE_6PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3f); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_NONE: + debug("MODE_NONE"); + + _pwm_default_rate = 10; /* artificially reduced output rate */ + _pwm_alt_rate = 10; + _pwm_alt_rate_channels = 0; + + /* disable servo outputs - no need to set rates */ + up_pwm_servo_deinit(); + + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) +{ + debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < _max_actuators; group++) { + + // get the channel mask for this rate group + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + // all channels in the group must be either default or alt-rate + uint32_t alt = rate_map & mask; + + if (pass == 0) { + // preflight + if ((alt != 0) && (alt != mask)) { + warn("rate group %u mask %x bad overlap %x", group, mask, alt); + // not a legal map, bail + return -EINVAL; + } + } else { + // set it - errors here are unexpected + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { + warn("rate group set alt failed"); + return -EINVAL; + } + } else { + if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { + warn("rate group set default failed"); + return -EINVAL; + } + } + } + } + } + _pwm_alt_rate_channels = rate_map; + _pwm_default_rate = default_rate; + _pwm_alt_rate = alt_rate; + + return OK; +} + +int +PX4FMU::set_pwm_alt_rate(unsigned rate) +{ + return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); +} + +int +PX4FMU::set_pwm_alt_channels(uint32_t channels) +{ + return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); +} + +void +PX4FMU::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + +#ifdef FMU_HAVE_PPM + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; +#endif + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + } + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; + } + + debug("adjusted actuator update interval to %ums", update_rate_in_ms); + orb_set_interval(_t_actuators, update_rate_in_ms); + + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; + } + + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; + } + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + + /* output to the servo */ + up_pwm_servo_set(i, outputs.output[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status if armed and not locked down */ + up_pwm_servo_arm(aa.armed && !aa.lockdown); + } + +#ifdef FMU_HAVE_PPM + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; icontrol[control_index]; + return 0; +} + +int +PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); + + /* try it as a GPIO ioctl first */ + ret = gpio_ioctl(filp, cmd, arg); + + if (ret != -ENOTTY) + return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch (_mode) { + case MODE_2PWM: + case MODE_4PWM: + case MODE_6PWM: + ret = pwm_ioctl(filp, cmd, arg); + break; + + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); + break; + + case PWM_SERVO_SET(5): + case PWM_SERVO_SET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(3): + case PWM_SERVO_SET(2): + if (_mode < MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(1): + case PWM_SERVO_SET(0): + if (arg < 2100) { + up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(3): + case PWM_SERVO_GET(2): + if (_mode < MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): + *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); + break; + + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + case PWM_SERVO_GET_RATEGROUP(4): + case PWM_SERVO_GET_RATEGROUP(5): + *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: + switch (_mode) { + case MODE_6PWM: + *(unsigned *)arg = 6; + break; + case MODE_4PWM: + *(unsigned *)arg = 4; + break; + case MODE_2PWM: + *(unsigned *)arg = 2; + break; + default: + ret = -EINVAL; + break; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +PX4FMU::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[6]; + + if (count > 6) { + // we have at most 6 outputs + count = 6; + } + + // allow for misaligned values + memcpy(values, buffer, count * 2); + + for (uint8_t i = 0; i < count; i++) { + up_pwm_servo_set(i, values[i]); + } + return count * 2; +} + +void +PX4FMU::gpio_reset(void) +{ + /* + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. + */ + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } + +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + /* if we have a GPIO direction control, set it to zero (input) */ + stm32_gpiowrite(GPIO_GPIO_DIR, 0); + stm32_configgpio(GPIO_GPIO_DIR); +#endif +} + +void +PX4FMU::gpio_set_function(uint32_t gpios, int function) +{ +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + /* + * GPIOs 0 and 1 must have the same direction as they are buffered + * by a shared 2-port driver. Any attempt to set either sets both. + */ + if (gpios & 3) { + gpios |= 3; + + /* flip the buffer to output mode if required */ + if (GPIO_SET_OUTPUT == function) + stm32_gpiowrite(GPIO_GPIO_DIR, 1); + } +#endif + + /* configure selected GPIOs as required */ + for (unsigned i = 0; i < _ngpio; i++) { + if (gpios & (1 << i)) { + switch (function) { + case GPIO_SET_INPUT: + stm32_configgpio(_gpio_tab[i].input); + break; + + case GPIO_SET_OUTPUT: + stm32_configgpio(_gpio_tab[i].output); + break; + + case GPIO_SET_ALT_1: + if (_gpio_tab[i].alt != 0) + stm32_configgpio(_gpio_tab[i].alt); + + break; + } + } + } + +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + /* flip buffer to input mode if required */ + if ((GPIO_SET_INPUT == function) && (gpios & 3)) + stm32_gpiowrite(GPIO_GPIO_DIR, 0); +#endif +} + +void +PX4FMU::gpio_write(uint32_t gpios, int function) +{ + int value = (function == GPIO_SET) ? 1 : 0; + + for (unsigned i = 0; i < _ngpio; i++) + if (gpios & (1 << i)) + stm32_gpiowrite(_gpio_tab[i].output, value); +} + +uint32_t +PX4FMU::gpio_read(void) +{ + uint32_t bits = 0; + + for (unsigned i = 0; i < _ngpio; i++) + if (stm32_gpioread(_gpio_tab[i].input)) + bits |= (1 << i); + + return bits; +} + +int +PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + + case GPIO_RESET: + gpio_reset(); + break; + + case GPIO_SET_OUTPUT: + case GPIO_SET_INPUT: + case GPIO_SET_ALT_1: + gpio_set_function(arg, cmd); + break; + + case GPIO_SET_ALT_2: + case GPIO_SET_ALT_3: + case GPIO_SET_ALT_4: + ret = -EINVAL; + break; + + case GPIO_SET: + case GPIO_CLEAR: + gpio_write(arg, cmd); + break; + + case GPIO_GET: + *(uint32_t *)arg = gpio_read(); + break; + + default: + ret = -ENOTTY; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNSET = 0, + PORT_FULL_GPIO, + PORT_FULL_SERIAL, + PORT_FULL_PWM, + PORT_GPIO_AND_SERIAL, + PORT_PWM_AND_SERIAL, + PORT_PWM_AND_GPIO, +}; + +PortMode g_port_mode; + +int +fmu_new_mode(PortMode new_mode) +{ + uint32_t gpio_bits; + PX4FMU::Mode servo_mode; + + /* reset to all-inputs */ + g_fmu->ioctl(0, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = PX4FMU::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT_FULL_PWM: +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + /* select 4-pin PWM mode */ + servo_mode = PX4FMU::MODE_4PWM; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + servo_mode = PX4FMU::MODE_6PWM; +#endif + break; + + /* mixed modes supported on v1 board only */ +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = PX4FMU::MODE_2PWM; + break; +#endif + default: + return -1; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_fmu->set_mode(servo_mode); + + return OK; +} + +int +fmu_start(void) +{ + int ret = OK; + + if (g_fmu == nullptr) { + + g_fmu = new PX4FMU; + + if (g_fmu == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_fmu->init(); + + if (ret != OK) { + delete g_fmu; + g_fmu = nullptr; + } + } + } + + return ret; +} + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); +#endif + + close(fd); + + exit(0); +} + +void +fake(int argc, char *argv[]) +{ + if (argc < 5) + errx(1, "fmu fake (values -100 .. 100)"); + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) + errx(1, "advertise failed"); + + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); + + exit(0); +} + +} // namespace + +extern "C" __EXPORT int fmu_main(int argc, char *argv[]); + +int +fmu_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + const char *verb = argv[1]; + + if (fmu_start() != OK) + errx(1, "failed to start the FMU driver"); + + /* + * Mode switches. + */ + if (!strcmp(verb, "mode_gpio")) { + new_mode = PORT_FULL_GPIO; + + } else if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT_FULL_PWM; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + + } else if (!strcmp(verb, "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; +#endif + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + int ret = fmu_new_mode(new_mode); + exit(ret == OK ? 0 : 1); + } + + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); + + fprintf(stderr, "FMU: unrecognised command, try:\n"); +#if defined(CONFIG_ARCH_BOARD_PX4FMU) + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); +#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) + fprintf(stderr, " mode_gpio, mode_pwm\n"); +#endif + exit(1); +} diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk new file mode 100644 index 000000000..05bc7a5b3 --- /dev/null +++ b/src/drivers/px4fmu/module.mk @@ -0,0 +1,6 @@ +# +# Interface driver for the PX4FMU board +# + +MODULE_COMMAND = fmu +SRCS = fmu.cpp diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk new file mode 100644 index 000000000..39b53ec9e --- /dev/null +++ b/src/drivers/rgbled/module.mk @@ -0,0 +1,6 @@ +# +# TCA62724FMG driver for RGB LED +# + +MODULE_COMMAND = rgbled +SRCS = rgbled.cpp diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp new file mode 100644 index 000000000..dc1e469c0 --- /dev/null +++ b/src/drivers/rgbled/rgbled.cpp @@ -0,0 +1,497 @@ +/**************************************************************************** + * + * 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 rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + * + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "device/rgbled.h" + +#define LED_ONTIME 120 +#define LED_OFFTIME 120 + +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +enum ledModes { + LED_MODE_TEST, + LED_MODE_SYSTEMSTATE, + LED_MODE_OFF +}; + +class RGBLED : public device::I2C +{ +public: + RGBLED(int bus, int rgbled); + virtual ~RGBLED(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int setMode(enum ledModes mode); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + enum ledColors { + LED_COLOR_OFF, + LED_COLOR_RED, + LED_COLOR_YELLOW, + LED_COLOR_PURPLE, + LED_COLOR_GREEN, + LED_COLOR_BLUE, + LED_COLOR_WHITE, + LED_COLOR_AMBER, + }; + + enum ledBlinkModes { + LED_BLINK_ON, + LED_BLINK_OFF + }; + + work_s _work; + + int led_colors[8]; + int led_blink; + + int mode; + int running; + + void setLEDColor(int ledcolor); + static void led_trampoline(void *arg); + void led(); + + int set(bool on, uint8_t r, uint8_t g, uint8_t b); + int set_on(bool on); + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); +}; + +/* for now, we only support one RGBLED */ +namespace +{ + RGBLED *g_rgbled; +} + + +extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); + +RGBLED::RGBLED(int bus, int rgbled) : + I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), + led_colors({0,0,0,0,0,0,0,0}), + led_blink(LED_BLINK_OFF), + mode(LED_MODE_OFF), + running(false) +{ + memset(&_work, 0, sizeof(_work)); +} + +RGBLED::~RGBLED() +{ +} + +int +RGBLED::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + /* start off */ + set(false, 0, 0, 0); + + return OK; +} + +int +RGBLED::setMode(enum ledModes new_mode) +{ + switch (new_mode) { + case LED_MODE_SYSTEMSTATE: + case LED_MODE_TEST: + mode = new_mode; + if (!running) { + running = true; + set_on(true); + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + break; + case LED_MODE_OFF: + + default: + if (running) { + running = false; + set_on(false); + } + mode = LED_MODE_OFF; + break; + } + + return OK; +} + +int +RGBLED::probe() +{ + int ret; + bool on, not_powersave; + uint8_t r, g, b; + + ret = get(on, not_powersave, r, g, b); + + return ret; +} + +int +RGBLED::info() +{ + int ret; + bool on, not_powersave; + uint8_t r, g, b; + + ret = get(on, not_powersave, r, g, b); + + if (ret == OK) { + /* we don't care about power-save mode */ + log("state: %s", on ? "ON" : "OFF"); + log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + } else { + warnx("failed to read led"); + } + + return ret; +} + +int +RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + + default: + break; + } + + return ret; +} + + +void +RGBLED::led_trampoline(void *arg) +{ + RGBLED *rgbl = (RGBLED *)arg; + + rgbl->led(); +} + + + +void +RGBLED::led() +{ + static int led_thread_runcount=0; + static int led_interval = 1000; + + switch (mode) { + case LED_MODE_TEST: + /* Demo LED pattern for now */ + led_colors[0] = LED_COLOR_YELLOW; + led_colors[1] = LED_COLOR_AMBER; + led_colors[2] = LED_COLOR_RED; + led_colors[3] = LED_COLOR_PURPLE; + led_colors[4] = LED_COLOR_BLUE; + led_colors[5] = LED_COLOR_GREEN; + led_colors[6] = LED_COLOR_WHITE; + led_colors[7] = LED_COLOR_OFF; + led_blink = LED_BLINK_ON; + break; + + case LED_MODE_SYSTEMSTATE: + /* XXX TODO set pattern */ + led_colors[0] = LED_COLOR_OFF; + led_colors[1] = LED_COLOR_OFF; + led_colors[2] = LED_COLOR_OFF; + led_colors[3] = LED_COLOR_OFF; + led_colors[4] = LED_COLOR_OFF; + led_colors[5] = LED_COLOR_OFF; + led_colors[6] = LED_COLOR_OFF; + led_colors[7] = LED_COLOR_OFF; + led_blink = LED_BLINK_OFF; + + break; + + case LED_MODE_OFF: + default: + return; + break; + } + + + if (led_thread_runcount & 1) { + if (led_blink == LED_BLINK_ON) + setLEDColor(LED_COLOR_OFF); + led_interval = LED_OFFTIME; + } else { + setLEDColor(led_colors[(led_thread_runcount/2) % 8]); + led_interval = LED_ONTIME; + } + + led_thread_runcount++; + + + if(running) { + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); + } else { + set_on(false); + } +} + +void RGBLED::setLEDColor(int ledcolor) { + switch (ledcolor) { + case LED_COLOR_OFF: // off + set_rgb(0,0,0); + break; + case LED_COLOR_RED: // red + set_rgb(255,0,0); + break; + case LED_COLOR_YELLOW: // yellow + set_rgb(255,70,0); + break; + case LED_COLOR_PURPLE: // purple + set_rgb(255,0,255); + break; + case LED_COLOR_GREEN: // green + set_rgb(0,255,0); + break; + case LED_COLOR_BLUE: // blue + set_rgb(0,0,255); + break; + case LED_COLOR_WHITE: // white + set_rgb(255,255,255); + break; + case LED_COLOR_AMBER: // amber + set_rgb(255,20,0); + break; + } +} + +int +RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) +{ + uint8_t settings_byte = 0; + + if (on) + settings_byte |= SETTING_ENABLE; +/* powersave not used */ +// if (not_powersave) + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::set_on(bool on) +{ + uint8_t settings_byte = 0; + + if (on) + settings_byte |= SETTING_ENABLE; + +/* powersave not used */ +// if (not_powersave) + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + + +int +RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2]; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + not_powersave = result[0] & SETTING_NOT_POWERSAVE; + /* XXX check, looks wrong */ + r = (result[0] & 0x0f)*255/15; + g = (result[1] & 0xf0)*255/15; + b = (result[1] & 0x0f)*255/15; + } + + return ret; +} + +void rgbled_usage(); + + +void rgbled_usage() { + warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --ddr addr (9)"); +} + +int +rgbled_main(int argc, char *argv[]) +{ + + int i2cdevice = PX4_I2C_BUS_LED; + int rgbledadr = ADDR; /* 7bit */ + + int x; + + for (x = 1; x < argc; x++) { + if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { + if (argc > x + 1) { + i2cdevice = atoi(argv[x + 1]); + } + } + + if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) { + if (argc > x + 1) { + rgbledadr = atoi(argv[x + 1]); + } + } + + } + + if (!strcmp(argv[1], "start")) { + if (g_rgbled != nullptr) + errx(1, "already started"); + + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) + errx(1, "new failed"); + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } + + exit(0); + } + + + if (g_rgbled == nullptr) { + fprintf(stderr, "not started\n"); + rgbled_usage(); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + g_rgbled->setMode(LED_MODE_TEST); + exit(0); + } + + if (!strcmp(argv[1], "systemstate")) { + g_rgbled->setMode(LED_MODE_SYSTEMSTATE); + exit(0); + } + + if (!strcmp(argv[1], "info")) { + g_rgbled->info(); + exit(0); + } + + if (!strcmp(argv[1], "off")) { + g_rgbled->setMode(LED_MODE_OFF); + exit(0); + } + + + /* things that require access to the device */ + int fd = open(RGBLED_DEVICE_PATH, 0); + if (fd < 0) + err(1, "can't open RGBLED device"); + + rgbled_usage(); + exit(0); +} -- cgit v1.2.3 From 4703a689798e2a520dfb69c0ef9146f3ecb956f2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Apr 2013 22:00:23 -0700 Subject: Fix the default state of the peripheral power control. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index cc4e9529d..78f6a2e65 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -89,7 +89,7 @@ __BEGIN_DECLS #define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* Power supply control and monitoring GPIOs */ -#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) #define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) -- cgit v1.2.3 From 4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 8 Apr 2013 17:04:04 +0400 Subject: position_estimator_inav: bugfixes, some refactoring --- .../position_estimator_inav_main.c | 80 ++++++++++++---------- .../position_estimator_inav_params.c | 23 +++++-- .../position_estimator_inav_params.h | 3 + 3 files changed, 63 insertions(+), 43 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 292cf7f21..97d669612 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -281,7 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -300,8 +299,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); + struct vehicle_local_position_s pos; + memset(&pos, 0, sizeof(pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -311,10 +310,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t local_pos_est_pub = orb_advertise( - ORB_ID(vehicle_local_position), &local_pos_est); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); - struct position_estimator_inav_params pos_inav_params; + struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); @@ -324,21 +322,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ /* FIRST PARAMETER UPDATE */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - if (use_gps) { - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, - .events = POLLIN } }; + if (params.use_gps) { + struct pollfd fds_init[1] = { + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; while (gps.fix_type < 3) { if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } static int printcounter = 0; @@ -354,6 +352,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; + + pos.home_lat = lat_current * 1e7; + pos.home_lon = lon_current * 1e7; + pos.home_timestamp = hrt_absolute_time(); + /* initialize coordinates */ map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ @@ -376,16 +379,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN } }; + struct pollfd fds[5] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; - int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ printf("[position_estimator_inav] subscriptions poll error\n"); @@ -399,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -442,7 +447,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } } - if (use_gps) { + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ @@ -454,8 +459,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { + pos.valid = true; } } /* end of poll return value check */ @@ -466,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; acceleration_correct(accel_corr, sensor.accelerometer_raw, - pos_inav_params.acc_T, pos_inav_params.acc_offs); + params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -491,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + kalman_filter_inertial_update(z_est, z_meas, params.k, use_z); } if (verbose_mode) { @@ -513,21 +521,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0f; - local_pos_est.vx = 0.0f; - local_pos_est.y = 0.0f; - local_pos_est.vy = 0.0f; - local_pos_est.z = z_est[0]; - local_pos_est.vz = z_est[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) - && (isfinite(local_pos_est.y)) - && (isfinite(local_pos_est.vy)) - && (isfinite(local_pos_est.z)) - && (isfinite(local_pos_est.vz))) { + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; + pos.vy = 0.0f; + pos.z = z_est[0]; + pos.vz = z_est[1]; + pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID( - vehicle_local_position), local_pos_est_pub, - &local_pos_est); + vehicle_local_position), vehicle_local_position_pub, + &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index fb082fbcb..d3a165947 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -42,6 +42,8 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ +PARAM_DEFINE_INT32(INAV_USE_GPS, 0); + PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); @@ -63,8 +65,9 @@ PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); -int parameters_init(struct position_estimator_inav_param_handles *h) -{ +int parameters_init(struct position_estimator_inav_param_handles *h) { + h->use_gps = param_find("INAV_USE_GPS"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); h->k_alt_01 = param_find("INAV_K_ALT_01"); h->k_alt_10 = param_find("INAV_K_ALT_10"); @@ -88,8 +91,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) -{ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { + param_get(h->use_gps, &(p->use_gps)); + param_get(h->k_alt_00, &(p->k[0][0])); param_get(h->k_alt_01, &(p->k[0][1])); param_get(h->k_alt_10, &(p->k[1][0])); @@ -97,9 +101,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_0, &(p->acc_offs[0])); - param_get(h->acc_offs_1, &(p->acc_offs[1])); - param_get(h->acc_offs_2, &(p->acc_offs[2])); + /* read int32 and cast to int16 */ + int32_t t; + param_get(h->acc_offs_0, &t); + p->acc_offs[0] = t; + param_get(h->acc_offs_1, &t); + p->acc_offs[1] = t; + param_get(h->acc_offs_2, &t); + p->acc_offs[2] = t; param_get(h->acc_t_00, &(p->acc_T[0][0])); param_get(h->acc_t_01, &(p->acc_T[0][1])); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 694bf015b..51e57a914 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -41,12 +41,15 @@ #include struct position_estimator_inav_params { + int use_gps; float k[3][2]; int16_t acc_offs[3]; float acc_T[3][3]; }; struct position_estimator_inav_param_handles { + param_t use_gps; + param_t k_alt_00; param_t k_alt_01; param_t k_alt_10; -- cgit v1.2.3 From 825957cde0e441ce89be2c44fccacd90c4514b59 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 8 Apr 2013 17:08:16 +0400 Subject: multirotor_pos_control: cleanup, altitude hold mode implemented --- .../multirotor_att_control_main.c | 190 ++++++++--------- .../multirotor_pos_control.c | 207 ++++++++++++------ .../multirotor_pos_control_params.c | 29 ++- .../multirotor_pos_control_params.h | 18 +- apps/multirotor_pos_control/position_control.c | 235 --------------------- apps/multirotor_pos_control/position_control.h | 50 ----- 6 files changed, 264 insertions(+), 465 deletions(-) delete mode 100644 apps/multirotor_pos_control/position_control.c delete mode 100644 apps/multirotor_pos_control/position_control.h diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c..09710f0fc 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -242,124 +242,124 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { - if (state.flag_control_attitude_enabled) { - - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { + + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; } - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; + static bool rc_loss_first_time = true; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; - rc_loss_first_time = false; + } else { + att_sp.thrust = 0.0f; + } - } else { - rc_loss_first_time = true; + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + rc_loss_first_time = false; - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; - } + } else { + rc_loss_first_time = true; - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; } - - control_yaw_position = true; } } - } - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); - } + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } - } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7b8d83aa8..d2b6685ac 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -35,7 +35,7 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include @@ -52,13 +52,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include "multirotor_pos_control_params.h" @@ -79,9 +82,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) -{ +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); @@ -96,21 +97,20 @@ usage(const char *reason) * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ +int multirotor_pos_control_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + printf("multirotor_pos_control already running\n"); /* this is not an error */ exit(0); } thread_should_exit = false; - deamon_task = task_spawn("multirotor pos control", + deamon_task = task_spawn("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, 4096, @@ -126,9 +126,9 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + printf("\tmultirotor_pos_control app is running\n"); } else { - printf("\tmultirotor pos control app not started\n"); + printf("\tmultirotor_pos_control app not started\n"); } exit(0); } @@ -137,98 +137,165 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) -{ +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + printf("[multirotor_pos_control] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct vehicle_attitude_s att; - //struct vehicle_global_position_setpoint_s global_pos_sp; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_vicon_position_s local_pos; - struct manual_control_setpoint_s manual; + memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - /* publish attitude setpoint */ + /* publish setpoint */ + orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - thread_running = true; + bool reset_sp_alt = true; + bool reset_sp_pos = true; + hrt_abstime t_prev = 0; + float alt_integral = 0.0f; + float alt_ctl_dz = 0.2f; - int loopcounter = 0; + thread_running = true; - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); + struct multirotor_position_control_params params; + struct multirotor_position_control_param_handles params_h; + parameters_init(¶ms_h); + parameters_update(¶ms_h, ¶ms); + int paramcheck_counter = 0; - while (1) { + while (!thread_should_exit) { /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + } - if (loopcounter == 500) { - parameters_update(&h, &p); - loopcounter = 0; + /* check parameters at 1 Hz*/ + paramcheck_counter++; + if (paramcheck_counter == 50) { + bool param_updated; + orb_check(param_sub, ¶m_updated); + if (param_updated) { + parameters_update(¶ms_h, ¶ms); + } + paramcheck_counter = 0; } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE - - float dT = 1.0f / 50.0f; - - float x_setpoint = 0.0f; - - // XXX enable switching between Vicon and local position estimate - /* local pos is the Vicon position */ - - // XXX just an example, lacks rotation around world-body transformation - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; - att_sp.roll_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { - /* set setpoint to current position */ - // XXX select pos reset channel on remote - /* reset setpoint to current position (position hold) */ - // if (1 == 2) { - // local_pos_sp.x = local_pos.x; - // local_pos_sp.y = local_pos.y; - // local_pos_sp.z = local_pos.z; - // local_pos_sp.yaw = att.yaw; - // } - // } + if (status.state_machine == SYSTEM_STATE_MANUAL) { + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } + + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + /* move altitude setpoint by manual controls */ + float alt_ctl = manual.throttle - 0.5f; + if (fabs(alt_ctl) < alt_ctl_dz) { + alt_ctl = 0.0f; + } else { + if (alt_ctl > 0.0f) + alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); + else + alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); + local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + err_linear_limit) + local_pos_sp.z = local_pos.z + err_linear_limit; + else if (local_pos_sp.z < local_pos.z - err_linear_limit) + local_pos_sp.z = local_pos.z - err_linear_limit; + } + + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* PID for altitude rate */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + float thrust_ctl = thrust_ctl_pd + alt_integral; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } else { + /* integrate only in linear area (with 20% gap) and not on min/max throttle */ + if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + // TODO add position controller + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } else { + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; + } else { + reset_sp_alt = true; + } + } else { + reset_sp_alt = true; + } /* run at approximately 50 Hz */ usleep(20000); - loopcounter++; } - printf("[multirotor pos control] ending now...\n"); + printf("[multirotor_pos_control] exiting\n"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); thread_running = false; diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/apps/multirotor_pos_control/multirotor_pos_control_params.c index 6b73dc405..90dd820f4 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.c +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.c @@ -34,29 +34,40 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c + * @file multirotor_pos_control_params.c * - * Parameters for EKF filter + * Parameters for multirotor_pos_control */ #include "multirotor_pos_control_params.h" -/* Extended Kalman Filter covariances */ - /* controller parameters */ -PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->thr_min = param_find("MPC_THR_MIN"); + h->thr_max = param_find("MPC_THR_MAX"); /* PID parameters */ - h->p = param_find("MC_POS_P"); - + h->alt_p = param_find("MPC_ALT_P"); + h->alt_i = param_find("MPC_ALT_I"); + h->alt_d = param_find("MPC_ALT_D"); + h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); return OK; } int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->p, &(p->p)); - + param_get(h->thr_min, &(p->thr_min)); + param_get(h->thr_max, &(p->thr_max)); + param_get(h->alt_p, &(p->alt_p)); + param_get(h->alt_i, &(p->alt_i)); + param_get(h->alt_d, &(p->alt_d)); + param_get(h->alt_rate_max, &(p->alt_rate_max)); return OK; } diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/apps/multirotor_pos_control/multirotor_pos_control_params.h index 274f6c22a..849055cd1 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.h +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.h @@ -42,15 +42,21 @@ #include struct multirotor_position_control_params { - float p; - float i; - float d; + float thr_min; + float thr_max; + float alt_p; + float alt_i; + float alt_d; + float alt_rate_max; }; struct multirotor_position_control_param_handles { - param_t p; - param_t i; - param_t d; + param_t thr_min; + param_t thr_max; + param_t alt_p; + param_t alt_i; + param_t alt_d; + param_t alt_rate_max; }; /** diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6..000000000 --- a/apps/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/apps/multirotor_pos_control/position_control.h b/apps/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc34..000000000 --- a/apps/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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 multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ -- cgit v1.2.3 From eb3d6f228cd96ef2a2b8a33f667dd1cdc2d5fdc5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 21:53:23 -0700 Subject: Added some functions for changing rates etc (WIP) --- src/drivers/lsm303d/lsm303d.cpp | 377 +++++++++++++++++++++++++++------------- 1 file changed, 257 insertions(+), 120 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 32030a1f7..ff56bff17 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -108,8 +108,10 @@ static const int ERROR = -1; #define ADDR_CTRL_REG3 0x22 #define ADDR_CTRL_REG4 0x23 #define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) @@ -127,11 +129,13 @@ static const int ERROR = -1; #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) +#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) #define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) #define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) #define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) #define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) +#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) #define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) #define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) #define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) @@ -143,6 +147,7 @@ static const int ERROR = -1; #define REG5_RES_HIGH_M ((1<<6) | (1<<5)) #define REG5_RES_LOW_M ((0<<6) | (0<<5)) +#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) #define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) #define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) #define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) @@ -151,6 +156,7 @@ static const int ERROR = -1; #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) #define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) +#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6)) #define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) #define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) #define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) @@ -174,72 +180,66 @@ public: LSM303D(int bus, const char* path, spi_dev_e device); virtual ~LSM303D(); - virtual int init(); + virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); - friend class LSM303D_mag; + friend class LSM303D_mag; virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); private: - LSM303D_mag *_mag; + LSM303D_mag *_mag; struct hrt_call _accel_call; struct hrt_call _mag_call; - unsigned _call_accel_interval; - unsigned _call_mag_interval; + unsigned _call_accel_interval; + unsigned _call_mag_interval; - unsigned _num_accel_reports; + unsigned _num_accel_reports; volatile unsigned _next_accel_report; volatile unsigned _oldest_accel_report; struct accel_report *_accel_reports; struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; + float _accel_range_scale; + float _accel_range_m_s2; orb_advert_t _accel_topic; - unsigned _num_mag_reports; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; struct mag_report *_mag_reports; struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; + float _mag_range_scale; + float _mag_range_ga; orb_advert_t _mag_topic; - unsigned _current_accel_rate; - unsigned _current_accel_range; - - unsigned _current_mag_rate; - unsigned _current_mag_range; - perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -250,24 +250,24 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void measure_trampoline(void *arg); + static void measure_trampoline(void *arg); /** * Static trampoline for the mag because it runs at a lower rate * * @param arg Instance pointer for the driver that is polling. */ - static void mag_measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void mag_measure(); /** * Read a register from the LSM303D @@ -275,7 +275,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the LSM303D @@ -283,7 +283,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the LSM303D @@ -294,27 +294,54 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D accel measurement range. + * + * @param max_g The measurement range of the accel is in g (9.81m/s^2) + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_g); /** - * Set the LSM303D measurement range. + * Set the LSM303D mag measurement range. * - * @param max_dps The measurement range is set to permit reading at least - * this rate in degrees per second. + * @param max_ga The measurement range of the mag is in Ga * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_dps); + int mag_set_range(unsigned max_g); + + /** + * Set the LSM303D accel anti-alias filter. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_antialias_filter_bandwidth(unsigned max_g); + + /** + * Set the LSM303D internal accel sampling frequency. + * + * @param frequency The internal accel sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); /** - * Set the LSM303D internal sampling frequency. + * Set the LSM303D internal mag sampling frequency. * - * @param frequency The internal sampling frequency is set to not less than + * @param frequency The internal mag sampling frequency is set to not less than * this value. * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int mag_set_samplerate(unsigned frequency); }; /** @@ -327,18 +354,18 @@ public: ~LSM303D_mag(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: friend class LSM303D; - void parent_poll_notify(); + void parent_poll_notify(); private: - LSM303D *_parent; + LSM303D *_parent; - void measure(); + void measure(); - void measure_trampoline(void *arg); + void measure_trampoline(void *arg); }; @@ -364,19 +391,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_reports(nullptr), _mag_range_scale(0.0f), _mag_range_ga(0.0f), - _current_accel_rate(0), - _current_accel_range(0), - _current_mag_rate(0), - _current_mag_range(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) { // enable debug() calls _debug_enabled = true; - /* XXX fix this default values */ - _accel_range_scale = 1.0f; - _mag_range_scale = 1.0f; + _mag_range_scale = 12.0f/32767.0f; // default scale factors _accel_scale.x_offset = 0; @@ -446,18 +467,21 @@ LSM303D::init() memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - /* XXX do this with ioctls */ - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + /* enable accel, XXX do this with an ioctl? */ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + + /* enable mag, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - /* XXX should we enable FIFO */ + /* XXX should we enable FIFO? */ - set_range(500); /* default to 500dps */ - set_samplerate(0); /* max sample rate */ + set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ + set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */ + set_samplerate(400); /* max sample rate */ -// _current_accel_rate = 100; + mag_set_range(12); /* XXX: take highest sensor range of 12GA? */ + mag_set_samplerate(100); /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ @@ -590,9 +614,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: + + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + case SENSOR_POLLRATE_DEFAULT: /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); + /* XXX for vibration tests with 800 Hz */ + return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* adjust to a legal polling interval in Hz */ default: { @@ -606,6 +634,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; + /* adjust sample rate of sensor */ + set_samplerate(arg); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _accel_call.period = _call_accel_interval = ticks; @@ -801,33 +832,111 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) } int -LSM303D::set_range(unsigned max_dps) +LSM303D::set_range(unsigned max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_FULL_SCALE_BITS_A; + float new_range = 0.0f; + + if (max_g == 0) + max_g = 16; + + if (max_g <= 2) { + new_range = 2.0f; + setbits |= REG2_FULL_SCALE_2G_A; + + } else if (max_g <= 4) { + new_range = 4.0f; + setbits |= REG2_FULL_SCALE_4G_A; + + } else if (max_g <= 6) { + new_range = 6.0f; + setbits |= REG2_FULL_SCALE_6G_A; + + } else if (max_g <= 8) { + new_range = 8.0f; + setbits |= REG2_FULL_SCALE_8G_A; + + } else if (max_g <= 16) { + new_range = 16.0f; + setbits |= REG2_FULL_SCALE_16G_A; + + } else { + return -EINVAL; + } + + _accel_range_m_s2 = new_range * 9.80665f; + _accel_range_scale = _accel_range_m_s2 / 32768.0f; + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_range(unsigned max_ga) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG6_FULL_SCALE_BITS_M; + float new_range = 0.0f; + + if (max_ga == 0) + max_ga = 12; + + if (max_ga <= 2) { + new_range = 2.0f; + setbits |= REG6_FULL_SCALE_2GA_M; + + } else if (max_ga <= 4) { + new_range = 4.0f; + setbits |= REG6_FULL_SCALE_4GA_M; + + } else if (max_ga <= 8) { + new_range = 8.0f; + setbits |= REG6_FULL_SCALE_8GA_M; + + } else if (max_ga <= 12) { + new_range = 12.0f; + setbits |= REG6_FULL_SCALE_12GA_M; + + } else { + return -EINVAL; + } + + _mag_range_ga = new_range; + _mag_range_scale = _mag_range_ga / 32768.0f; + + modify_reg(ADDR_CTRL_REG6, clearbits, setbits); + + return OK; +} + +int +LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) { - /* XXX implement this */ -// uint8_t bits = REG4_BDU; -// -// if (max_dps == 0) -// max_dps = 2000; -// -// if (max_dps <= 250) { -// _current_range = 250; -// bits |= RANGE_250DPS; -// -// } else if (max_dps <= 500) { -// _current_range = 500; -// bits |= RANGE_500DPS; -// -// } else if (max_dps <= 2000) { -// _current_range = 2000; -// bits |= RANGE_2000DPS; -// -// } else { -// return -EINVAL; -// } -// -// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; -// _gyro_range_scale = _gyro_range_rad_s / 32768.0f; -// write_reg(ADDR_CTRL_REG4, bits); + uint8_t setbits = 0; + uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; + + if (bandwidth == 0) + bandwidth = 773; + + if (bandwidth <= 50) { + setbits |= REG2_AA_FILTER_BW_50HZ_A; + + } else if (bandwidth <= 194) { + setbits |= REG2_AA_FILTER_BW_194HZ_A; + + } else if (bandwidth <= 362) { + setbits |= REG2_AA_FILTER_BW_362HZ_A; + + } else if (bandwidth <= 773) { + setbits |= REG2_AA_FILTER_BW_773HZ_A; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); return OK; } @@ -835,33 +944,60 @@ LSM303D::set_range(unsigned max_dps) int LSM303D::set_samplerate(unsigned frequency) { - /* XXX implement this */ -// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; -// -// if (frequency == 0) -// frequency = 760; -// -// if (frequency <= 95) { -// _current_rate = 95; -// bits |= RATE_95HZ_LP_25HZ; -// -// } else if (frequency <= 190) { -// _current_rate = 190; -// bits |= RATE_190HZ_LP_25HZ; -// -// } else if (frequency <= 380) { -// _current_rate = 380; -// bits |= RATE_380HZ_LP_30HZ; -// -// } else if (frequency <= 760) { -// _current_rate = 760; -// bits |= RATE_760HZ_LP_30HZ; -// -// } else { -// return -EINVAL; -// } -// -// write_reg(ADDR_CTRL_REG1, bits); + uint8_t setbits = 0; + uint8_t clearbits = REG1_RATE_BITS_A; + + if (frequency == 0) + frequency = 1600; + + if (frequency <= 100) { + setbits |= REG1_RATE_100HZ_A; + + } else if (frequency <= 200) { + setbits |= REG1_RATE_200HZ_A; + + } else if (frequency <= 400) { + setbits |= REG1_RATE_400HZ_A; + + } else if (frequency <= 800) { + setbits |= REG1_RATE_800HZ_A; + + } else if (frequency <= 1600) { + setbits |= REG1_RATE_1600HZ_A; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG5_RATE_BITS_M; + + if (frequency == 0) + frequency = 100; + + if (frequency <= 25) { + setbits |= REG5_RATE_25HZ_M; + + } else if (frequency <= 50) { + setbits |= REG5_RATE_50HZ_M; + + } else if (frequency <= 100) { + setbits |= REG5_RATE_100HZ_M; + + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG5, clearbits, setbits); return OK; } @@ -956,8 +1092,8 @@ LSM303D::measure() accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; + accel_report->scaling = _accel_range_scale; + accel_report->range_m_s2 = _accel_range_m_s2; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); @@ -1183,15 +1319,13 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); - /* XXX fix the test output */ -// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); -// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); -// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); -// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); - + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); int fd_mag = -1; @@ -1209,10 +1343,13 @@ test() if (sz != sizeof(m_report)) err(1, "immediate read failed"); - /* XXX fix the test output */ + warnx("mag x: \t% 9.5f\tga", (double)m_report.x); + warnx("mag y: \t% 9.5f\tga", (double)m_report.y); + warnx("mag z: \t% 9.5f\tga", (double)m_report.z); warnx("mag x: \t%d\traw", (int)m_report.x_raw); warnx("mag y: \t%d\traw", (int)m_report.y_raw); warnx("mag z: \t%d\traw", (int)m_report.z_raw); + warnx("mag range: %8.4f ga", (double)m_report.range_ga); /* XXX add poll-rate tests here too */ -- cgit v1.2.3 From 1d327c42a6f34f8545940cd8630586726e4d3ae6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 22:04:21 -0700 Subject: Mag sample rate was not actually changed by an ioctl --- src/drivers/lsm303d/lsm303d.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ff56bff17..b107b869f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -718,7 +718,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 50 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 50); + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { @@ -732,12 +732,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; + /* adjust sample rate of sensor */ + mag_set_samplerate(arg); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; - - /* if we need to start the poll state machine, do it */ if (want_start) start(); -- cgit v1.2.3 From 3469fefe117f8fa3bfef3fbcb3751a32e81c324a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 23:47:19 -0700 Subject: Checked axes of LSM303D --- src/drivers/lsm303d/lsm303d.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b107b869f..8a64ee702 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1067,7 +1067,6 @@ LSM303D::measure() raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); - /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1085,7 +1084,7 @@ LSM303D::measure() accel_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ + accel_report->x_raw = raw_accel_report.x; accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; @@ -1136,7 +1135,6 @@ LSM303D::mag_measure() raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); - /* XXX adapt the comment to specs */ /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) @@ -1154,15 +1152,15 @@ LSM303D::mag_measure() mag_report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ + mag_report->x_raw = raw_mag_report.x; mag_report->y_raw = raw_mag_report.y; mag_report->z_raw = raw_mag_report.z; mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; -// report->scaling = _gyro_range_scale; -// report->range_rad_s = _gyro_range_rad_s; + mag_report->scaling = _mag_range_scale; + mag_report->range_ga = _mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); -- cgit v1.2.3 From 1a8cca92e9a7b8fad153042a44c1754f31a2745b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 8 Apr 2013 23:29:24 -0700 Subject: Fixed axis in L3GD20 driver --- apps/drivers/l3gd20/l3gd20.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index 6227df72a..c7f433dd4 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -684,9 +684,10 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report->timestamp = hrt_absolute_time(); - /* XXX adjust for sensor alignment to board here */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -- cgit v1.2.3 From 64ec950c58c693cf1a9c0c5feadcee96cc90c4cd Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Sun, 14 Apr 2013 20:53:42 -0700 Subject: px4iov2 nsh boots --- apps/drivers/boards/px4iov2/Makefile | 41 ++ apps/drivers/boards/px4iov2/module.mk | 5 + apps/drivers/boards/px4iov2/px4iov2_init.c | 172 ++++++ apps/drivers/boards/px4iov2/px4iov2_internal.h | 135 +++++ nuttx/configs/px4iov2/README.txt | 806 +++++++++++++++++++++++++ nuttx/configs/px4iov2/common/Make.defs | 175 ++++++ nuttx/configs/px4iov2/common/ld.script | 129 ++++ nuttx/configs/px4iov2/common/setenv.sh | 47 ++ nuttx/configs/px4iov2/include/README.txt | 1 + nuttx/configs/px4iov2/include/board.h | 173 ++++++ nuttx/configs/px4iov2/include/drv_i2c_device.h | 42 ++ nuttx/configs/px4iov2/io/Make.defs | 3 + nuttx/configs/px4iov2/io/appconfig | 40 ++ nuttx/configs/px4iov2/io/defconfig | 548 +++++++++++++++++ nuttx/configs/px4iov2/io/setenv.sh | 47 ++ nuttx/configs/px4iov2/nsh/Make.defs | 3 + nuttx/configs/px4iov2/nsh/appconfig | 44 ++ nuttx/configs/px4iov2/nsh/defconfig | 567 +++++++++++++++++ nuttx/configs/px4iov2/nsh/setenv.sh | 47 ++ nuttx/configs/px4iov2/src/Makefile | 84 +++ nuttx/configs/px4iov2/src/README.txt | 1 + nuttx/configs/px4iov2/src/drv_i2c_device.c | 618 +++++++++++++++++++ nuttx/configs/px4iov2/src/empty.c | 4 + 23 files changed, 3732 insertions(+) create mode 100644 apps/drivers/boards/px4iov2/Makefile create mode 100644 apps/drivers/boards/px4iov2/module.mk create mode 100644 apps/drivers/boards/px4iov2/px4iov2_init.c create mode 100644 apps/drivers/boards/px4iov2/px4iov2_internal.h create mode 100755 nuttx/configs/px4iov2/README.txt create mode 100644 nuttx/configs/px4iov2/common/Make.defs create mode 100755 nuttx/configs/px4iov2/common/ld.script create mode 100755 nuttx/configs/px4iov2/common/setenv.sh create mode 100755 nuttx/configs/px4iov2/include/README.txt create mode 100755 nuttx/configs/px4iov2/include/board.h create mode 100644 nuttx/configs/px4iov2/include/drv_i2c_device.h create mode 100644 nuttx/configs/px4iov2/io/Make.defs create mode 100644 nuttx/configs/px4iov2/io/appconfig create mode 100755 nuttx/configs/px4iov2/io/defconfig create mode 100755 nuttx/configs/px4iov2/io/setenv.sh create mode 100644 nuttx/configs/px4iov2/nsh/Make.defs create mode 100644 nuttx/configs/px4iov2/nsh/appconfig create mode 100755 nuttx/configs/px4iov2/nsh/defconfig create mode 100755 nuttx/configs/px4iov2/nsh/setenv.sh create mode 100644 nuttx/configs/px4iov2/src/Makefile create mode 100644 nuttx/configs/px4iov2/src/README.txt create mode 100644 nuttx/configs/px4iov2/src/drv_i2c_device.c create mode 100644 nuttx/configs/px4iov2/src/empty.c diff --git a/apps/drivers/boards/px4iov2/Makefile b/apps/drivers/boards/px4iov2/Makefile new file mode 100644 index 000000000..85806fe6f --- /dev/null +++ b/apps/drivers/boards/px4iov2/Makefile @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4IO +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4io + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4iov2/module.mk b/apps/drivers/boards/px4iov2/module.mk new file mode 100644 index 000000000..d596ce4db --- /dev/null +++ b/apps/drivers/boards/px4iov2/module.mk @@ -0,0 +1,5 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c diff --git a/apps/drivers/boards/px4iov2/px4iov2_init.c b/apps/drivers/boards/px4iov2/px4iov2_init.c new file mode 100644 index 000000000..711bee425 --- /dev/null +++ b/apps/drivers/boards/px4iov2/px4iov2_init.c @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * 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 px4iov2_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "stm32_internal.h" +#include "px4iov2_internal.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + + /* configure GPIOs */ + + /* turn off - all leds are active low */ + stm32_gpiowrite(BOARD_GPIO_LED1, true); + stm32_gpiowrite(BOARD_GPIO_LED2, true); + stm32_gpiowrite(BOARD_GPIO_LED3, true); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED1)); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED2)); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED3)); + + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_BTN_SAFETY)); + + /* spektrum power enable is active high - disable it by default */ + stm32_gpiowrite(BOARD_GPIO_SPEKTRUM_PWR_EN, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SPEKTRUM_PWR_EN)); + + /* servo power enable is active low, and has a pull down resistor + * to keep it low during boot (since it may power the whole board.) + */ + stm32_gpiowrite(BOARD_GPIO_SERVO_PWR_EN, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SERVO_PWR_EN)); + + stm32_configgpio(BOARD_GPIO_INPUT_PUP(BOARD_GPIO_SERVO_FAULT_DETECT)); + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_TIM_RSSI)); /* xxx alternate function */ + stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_RSSI)); + stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_VSERVO)); + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_SBUS_INPUT)); /* xxx alternate function */ + + stm32_gpiowrite(BOARD_GPIO_SBUS_OUTPUT, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OUTPUT)); + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(BOARD_GPIO_SBUS_OENABLE, true); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OENABLE)); + + + stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_PPM)); /* xxx alternate function */ + + stm32_gpiowrite(BOARD_GPIO_PWM1, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM1)); + + stm32_gpiowrite(BOARD_GPIO_PWM2, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM2)); + + stm32_gpiowrite(BOARD_GPIO_PWM3, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM3)); + + stm32_gpiowrite(BOARD_GPIO_PWM4, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM4)); + + stm32_gpiowrite(BOARD_GPIO_PWM5, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM5)); + + stm32_gpiowrite(BOARD_GPIO_PWM6, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM6)); + + stm32_gpiowrite(BOARD_GPIO_PWM7, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM7)); + + stm32_gpiowrite(BOARD_GPIO_PWM8, false); + stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM8)); + +// message("[boot] Successfully initialized px4iov2 gpios\n"); + + return OK; +} diff --git a/apps/drivers/boards/px4iov2/px4iov2_internal.h b/apps/drivers/boards/px4iov2/px4iov2_internal.h new file mode 100644 index 000000000..9675c6f36 --- /dev/null +++ b/apps/drivers/boards/px4iov2/px4iov2_internal.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +#define BOARD_GPIO_OUTPUT(pin) (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|(pin)) +#define BOARD_GPIO_INPUT_FLOAT(pin) (GPIO_INPUT|GPIO_CNF_INFLOAT|\ + GPIO_MODE_INPUT|(pin)) +#define BOARD_GPIO_INPUT_PUP(pin) (GPIO_INPUT|GPIO_CNF_INPULLUP|\ + GPIO_MODE_INPUT|(pin)) +#define BOARD_GPIO_INPUT_ANALOG(pin) (GPIO_INPUT|GPIO_CNF_ANALOGIN|\ + GPIO_MODE_INPUT|(pin)) + +/* LEDS **********************************************************************/ + +#define BOARD_GPIO_LED1 (GPIO_PORTB|GPIO_PIN14) +#define BOARD_GPIO_LED2 (GPIO_PORTB|GPIO_PIN15) +#define BOARD_GPIO_LED3 (GPIO_PORTB|GPIO_PIN13) + +#define BOARD_GPIO_LED_BLUE BOARD_GPIO_LED1 +#define BOARD_GPIO_LED_AMBER BOARD_GPIO_LED2 +#define BOARD_GPIO_LED_SAFETY BOARD_GPIO_LED3 + +/* Safety switch button *******************************************************/ + +#define BOARD_GPIO_BTN_SAFETY (GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define BOARD_GPIO_SPEKTRUM_PWR_EN (GPIO_PORTC|GPIO_PIN13) + +#define BOARD_GPIO_SERVO_PWR_EN (GPIO_PORTC|GPIO_PIN15) + +#define BOARD_GPIO_SERVO_FAULT_DETECT (GPIO_PORTB|GPIO_PIN13) + + +/* Analog inputs **************************************************************/ + +#define BOARD_GPIO_ADC_VSERVO (GPIO_PORTA|GPIO_PIN4) +/* the same rssi signal goes to both an adc and a timer input */ +#define BOARD_GPIO_ADC_RSSI (GPIO_PORTA|GPIO_PIN5) +#define BOARD_GPIO_TIM_RSSI (GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define BOARD_GPIO_PPM (GPIO_PORTA|GPIO_PIN8) + +#define BOARD_GPIO_PWM1 (GPIO_PORTA|GPIO_PIN0) +#define BOARD_GPIO_PWM2 (GPIO_PORTA|GPIO_PIN1) +#define BOARD_GPIO_PWM3 (GPIO_PORTB|GPIO_PIN8) +#define BOARD_GPIO_PWM4 (GPIO_PORTB|GPIO_PIN9) +#define BOARD_GPIO_PWM5 (GPIO_PORTA|GPIO_PIN6) +#define BOARD_GPIO_PWM6 (GPIO_PORTA|GPIO_PIN7) +#define BOARD_GPIO_PWM7 (GPIO_PORTB|GPIO_PIN0) +#define BOARD_GPIO_PWM8 (GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +#define BOARD_GPIO_SBUS_INPUT (GPIO_PORTB|GPIO_PIN11) +#define BOARD_GPIO_SBUS_OUTPUT (GPIO_PORTB|GPIO_PIN10) +#define BOARD_GPIO_SBUS_OENABLE (GPIO_PORTB|GPIO_PIN4) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/nuttx/configs/px4iov2/README.txt b/nuttx/configs/px4iov2/README.txt new file mode 100755 index 000000000..9b1615f42 --- /dev/null +++ b/nuttx/configs/px4iov2/README.txt @@ -0,0 +1,806 @@ +README +====== + +This README discusses issues unique to NuttX configurations for the +STMicro STM3210E-EVAL development board. + +Contents +======== + + - Development Environment + - GNU Toolchain Options + - IDEs + - NuttX buildroot Toolchain + - DFU and JTAG + - OpenOCD + - LEDs + - Temperature Sensor + - RTC + - STM3210E-EVAL-specific Configuration Options + - Configurations + +Development Environment +======================= + + Either Linux or Cygwin on Windows can be used for the development environment. + The source has been built only using the GNU toolchain (see below). Other + toolchains will likely cause problems. Testing was performed using the Cygwin + environment because the Raisonance R-Link emulatator and some RIDE7 development tools + were used and those tools works only under Windows. + +GNU Toolchain Options +===================== + + The NuttX make system has been modified to support the following different + toolchain options. + + 1. The CodeSourcery GNU toolchain, + 2. The devkitARM GNU toolchain, + 3. Raisonance GNU toolchain, or + 4. The NuttX buildroot Toolchain (see below). + + All testing has been conducted using the NuttX buildroot toolchain. However, + the make system is setup to default to use the devkitARM toolchain. To use + the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to + add one of the following configuration options to your .config (or defconfig) + file: + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux + CONFIG_STM32_DEVKITARM=y : devkitARM under Windows + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) + + If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify + the PATH in the setenv.h file if your make cannot find the tools. + + NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are + Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot + toolchains are Cygwin and/or Linux native toolchains. There are several limitations + to using a Windows based toolchain in a Cygwin environment. The three biggest are: + + 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are + performed automatically in the Cygwin makefiles using the 'cygpath' utility + but you might easily find some new path problems. If so, check out 'cygpath -w' + + 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links + are used in Nuttx (e.g., include/arch). The make system works around these + problems for the Windows tools by copying directories instead of linking them. + But this can also cause some confusion for you: For example, you may edit + a file in a "linked" directory and find that your changes had no effect. + That is because you are building the copy of the file in the "fake" symbolic + directory. If you use a Windows toolchain, you should get in the habit of + making like this: + + make clean_context all + + An alias in your .bashrc file might make that less painful. + + 3. Dependencies are not made when using Windows versions of the GCC. This is + because the dependencies are generated using Windows pathes which do not + work with the Cygwin make. + + Support has been added for making dependencies with the windows-native toolchains. + That support can be enabled by modifying your Make.defs file as follows: + + - MKDEP = $(TOPDIR)/tools/mknulldeps.sh + + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" + + If you have problems with the dependency build (for example, if you are not + building on C:), then you may need to modify tools/mkdeps.sh + + NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization + level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with + -Os. + + NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that + the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM + path or will get the wrong version of make. + +IDEs +==== + + NuttX is built using command-line make. It can be used with an IDE, but some + effort will be required to create the project (There is a simple RIDE project + in the RIDE subdirectory). + + Makefile Build + -------------- + Under Eclipse, it is pretty easy to set up an "empty makefile project" and + simply use the NuttX makefile to build the system. That is almost for free + under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty + makefile project in order to work with Windows (Google for "Eclipse Cygwin" - + there is a lot of help on the internet). + + Native Build + ------------ + Here are a few tips before you start that effort: + + 1) Select the toolchain that you will be using in your .config file + 2) Start the NuttX build at least one time from the Cygwin command line + before trying to create your project. This is necessary to create + certain auto-generated files and directories that will be needed. + 3) Set up include pathes: You will need include/, arch/arm/src/stm32, + arch/arm/src/common, arch/arm/src/armv7-m, and sched/. + 4) All assembly files need to have the definition option -D __ASSEMBLY__ + on the command line. + + Startup files will probably cause you some headaches. The NuttX startup file + is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX + one time from the Cygwin command line in order to obtain the pre-built + startup object needed by RIDE. + +NuttX buildroot Toolchain +========================= + + A GNU GCC-based toolchain is assumed. The files */setenv.sh should + be modified to point to the correct path to the Cortex-M3 GCC toolchain (if + different from the default in your PATH variable). + + If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX + SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). + This GNU toolchain builds and executes in the Linux or Cygwin environment. + + 1. You must have already configured Nuttx in /nuttx. + + cd tools + ./configure.sh stm3210e-eval/ + + 2. Download the latest buildroot package into + + 3. unpack the buildroot tarball. The resulting directory may + have versioning information on it like buildroot-x.y.z. If so, + rename /buildroot-x.y.z to /buildroot. + + 4. cd /buildroot + + 5. cp configs/cortexm3-defconfig-4.3.3 .config + + 6. make oldconfig + + 7. make + + 8. Edit setenv.h, if necessary, so that the PATH variable includes + the path to the newly built binaries. + + See the file configs/README.txt in the buildroot source tree. That has more + detailed PLUS some special instructions that you will need to follow if you are + building a Cortex-M3 toolchain for Cygwin under Windows. + +DFU and JTAG +============ + + Enbling Support for the DFU Bootloader + -------------------------------------- + The linker files in these projects can be configured to indicate that you + will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) + loader or via some JTAG emulator. You can specify the DFU bootloader by + adding the following line: + + CONFIG_STM32_DFU=y + + to your .config file. Most of the configurations in this directory are set + up to use the DFU loader. + + If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning + of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed + to make space for the DFU loader and 0x08003000 is where the DFU loader expects + to find new applications at boot time. If you need to change that origin for some + other bootloader, you will need to edit the file(s) ld.script.dfu for each + configuration. + + The DFU SE PC-based software is available from the STMicro website, + http://www.st.com. General usage instructions: + + 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU + file (nuttx.dfu)... see below for details. + 2. Connect the STM3210E-EVAL board to your computer using a USB + cable. + 3. Start the DFU loader on the STM3210E-EVAL board. You do this by + resetting the board while holding the "Key" button. Windows should + recognize that the DFU loader has been installed. + 3. Run the DFU SE program to load nuttx.dfu into FLASH. + + What if the DFU loader is not in FLASH? The loader code is available + inside of the Demo dirctory of the USBLib ZIP file that can be downloaded + from the STMicro Website. You can build it using RIDE (or other toolchains); + you will need a JTAG emulator to burn it into FLASH the first time. + + In order to use STMicro's built-in DFU loader, you will have to get + the NuttX binary into a special format with a .dfu extension. The + DFU SE PC_based software installation includes a file "DFU File Manager" + conversion program that a file in Intel Hex format to the special DFU + format. When you successfully build NuttX, you will find a file called + nutt.ihx in the top-level directory. That is the file that you should + provide to the DFU File Manager. You will need to rename it to nuttx.hex + in order to find it with the DFU File Manager. You will end up with + a file called nuttx.dfu that you can use with the STMicro DFU SE program. + + Enabling JTAG + ------------- + If you are not using the DFU, then you will probably also need to enable + JTAG support. By default, all JTAG support is disabled but there NuttX + configuration options to enable JTAG in various different ways. + + These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO + MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the + Cortex debug port. The default state in this port is for all JTAG support + to be disable. + + CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full + SWJ (JTAG-DP + SW-DP) + + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable + full SWJ (JTAG-DP + SW-DP) but without JNTRST. + + CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP + disabled and SW-DP enabled + + The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 + which disable JTAG-DP and SW-DP. + +OpenOCD +======= + +I have also used OpenOCD with the STM3210E-EVAL. In this case, I used +the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh +for more information. Using the script: + +1) Start the OpenOCD GDB server + + cd + configs/stm3210e-eval/tools/oocd.sh $PWD + +2) Load Nuttx + + cd + arm-none-eabi-gdb nuttx + gdb> target remote localhost:3333 + gdb> mon reset + gdb> mon halt + gdb> load nuttx + +3) Running NuttX + + gdb> mon reset + gdb> c + +LEDs +==== + +The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the +board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is +defined. In that case, the usage by the board port is defined in +include/board.h and src/up_leds.c. The LEDs are used to encode OS-related +events as follows: + + SYMBOL Meaning LED1* LED2 LED3 LED4 + ---------------- ----------------------- ----- ----- ----- ----- + LED_STARTED NuttX has been started ON OFF OFF OFF + LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF + LED_IRQSENABLED Interrupts enabled ON ON OFF OFF + LED_STACKCREATED Idle stack created OFF OFF ON OFF + LED_INIRQ In an interrupt** ON N/C N/C OFF + LED_SIGNAL In a signal handler*** N/C ON N/C OFF + LED_ASSERTION An assertion failed ON ON N/C OFF + LED_PANIC The system has crashed N/C N/C N/C ON + LED_IDLE STM32 is is sleep mode (Optional, not used) + + * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot + and these LEDs will give you some indication of where the failure was + ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + is because of timer interupts that result in the LED being illuminated + on a small proportion of the time. +*** LED2 may also flicker normally if signals are processed. + +Temperature Sensor +================== + +Support for the on-board LM-75 temperature sensor is available. This supported +has been verified, but has not been included in any of the available the +configurations. To set up the temperature sensor, add the following to the +NuttX configuration file + + CONFIG_I2C=y + CONFIG_I2C_LM75=y + +Then you can implement logic like the following to use the temperature sensor: + + #include + #include + + ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ + fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ + ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ + bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ + +More complex temperature sensor operations are also available. See the IOCTAL +commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions +of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the +arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). + +RTC +=== + + The STM32 RTC may configured using the following settings. + + CONFIG_RTC - Enables general support for a hardware RTC. Specific + architectures may require other specific settings. + CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 + second, usually supporting a 32-bit time_t value. In this case, + the RTC is used to "seed" the normal NuttX timer and the + NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES + is enabled in the NuttX configuration, then the RTC provides higher + resolution time and completely replaces the system timer for purpose of + date and time. + CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the + frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES + is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. + CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. + A callback function will be executed when the alarm goes off + + In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts + are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. + A BKP register is incremented on each overflow interrupt creating, effectively, + a 48-bit RTC counter. + + In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled + (because the next overflow is not expected until the year 2106. + + WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The + overflow interrupt may be lost even if the STM32 is powered down only momentarily. + Therefore hi-res solution is only useful in systems where the power is always on. + +STM3210E-EVAL-specific Configuration Options +============================================ + + CONFIG_ARCH - Identifies the arch/ subdirectory. This should + be set to: + + CONFIG_ARCH=arm + + CONFIG_ARCH_family - For use in C code: + + CONFIG_ARCH_ARM=y + + CONFIG_ARCH_architecture - For use in C code: + + CONFIG_ARCH_CORTEXM3=y + + CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory + + CONFIG_ARCH_CHIP=stm32 + + CONFIG_ARCH_CHIP_name - For use in C code to identify the exact + chip: + + CONFIG_ARCH_CHIP_STM32F103ZET6 + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock + configuration features. + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n + + CONFIG_ARCH_BOARD - Identifies the configs subdirectory and + hence, the board that supports the particular chip or SoC. + + CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) + + CONFIG_ARCH_BOARD_name - For use in C code + + CONFIG_ARCH_BOARD_STM3210E_EVAL=y + + CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation + of delay loops + + CONFIG_ENDIAN_BIG - define if big endian (default is little + endian) + + CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): + + CONFIG_DRAM_SIZE=0x00010000 (64Kb) + + CONFIG_DRAM_START - The start address of installed DRAM + + CONFIG_DRAM_START=0x20000000 + + CONFIG_DRAM_END - Last address+1 of installed RAM + + CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) + + CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization + + CONFIG_ARCH_IRQPRIO=y + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that + have LEDs + + CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt + stack. If defined, this symbol is the size of the interrupt + stack in bytes. If not defined, the user task stacks will be + used during interrupt handling. + + CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. + + CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that + cause a 100 second delay during boot-up. This 100 second delay + serves no purpose other than it allows you to calibratre + CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure + the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until + the delay actually is 100 seconds. + + Individual subsystems can be enabled: + AHB + --- + CONFIG_STM32_DMA1 + CONFIG_STM32_DMA2 + CONFIG_STM32_CRC + CONFIG_STM32_FSMC + CONFIG_STM32_SDIO + + APB1 + ---- + CONFIG_STM32_TIM2 + CONFIG_STM32_TIM3 + CONFIG_STM32_TIM4 + CONFIG_STM32_TIM5 + CONFIG_STM32_TIM6 + CONFIG_STM32_TIM7 + CONFIG_STM32_WWDG + CONFIG_STM32_SPI2 + CONFIG_STM32_SPI4 + CONFIG_STM32_USART2 + CONFIG_STM32_USART3 + CONFIG_STM32_UART4 + CONFIG_STM32_UART5 + CONFIG_STM32_I2C1 + CONFIG_STM32_I2C2 + CONFIG_STM32_USB + CONFIG_STM32_CAN + CONFIG_STM32_BKP + CONFIG_STM32_PWR + CONFIG_STM32_DAC1 + CONFIG_STM32_DAC2 + CONFIG_STM32_USB + + APB2 + ---- + CONFIG_STM32_ADC1 + CONFIG_STM32_ADC2 + CONFIG_STM32_TIM1 + CONFIG_STM32_SPI1 + CONFIG_STM32_TIM8 + CONFIG_STM32_USART1 + CONFIG_STM32_ADC3 + + Timer and I2C devices may need to the following to force power to be applied + unconditionally at power up. (Otherwise, the device is powered when it is + initialized). + + CONFIG_STM32_FORCEPOWER + + Timer devices may be used for different purposes. One special purpose is + to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn + is defined (as above) then the following may also be defined to indicate that + the timer is intended to be used for pulsed output modulation, ADC conversion, + or DAC conversion. Note that ADC/DAC require two definition: Not only do you have + to assign the timer (n) for used by the ADC or DAC, but then you also have to + configure which ADC or DAC (m) it is assigned to. + + CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 + CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 + CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 + CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 + CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 + + For each timer that is enabled for PWM usage, we need the following additional + configuration settings: + + CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} + + NOTE: The STM32 timers are each capable of generating different signals on + each of the four channels with different duty cycles. That capability is + not supported by this driver: Only one output channel per timer. + + Alternate pin mappings (should not be used with the STM3210E-EVAL board): + + CONFIG_STM32_TIM1_FULL_REMAP + CONFIG_STM32_TIM1_PARTIAL_REMAP + CONFIG_STM32_TIM2_FULL_REMAP + CONFIG_STM32_TIM2_PARTIAL_REMAP_1 + CONFIG_STM32_TIM2_PARTIAL_REMAP_2 + CONFIG_STM32_TIM3_FULL_REMAP + CONFIG_STM32_TIM3_PARTIAL_REMAP + CONFIG_STM32_TIM4_REMAP + CONFIG_STM32_USART1_REMAP + CONFIG_STM32_USART2_REMAP + CONFIG_STM32_USART3_FULL_REMAP + CONFIG_STM32_USART3_PARTIAL_REMAP + CONFIG_STM32_SPI1_REMAP + CONFIG_STM32_SPI3_REMAP + CONFIG_STM32_I2C1_REMAP + CONFIG_STM32_CAN1_FULL_REMAP + CONFIG_STM32_CAN1_PARTIAL_REMAP + CONFIG_STM32_CAN2_REMAP + + JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): + CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + but without JNTRST. + CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled + + STM32F103Z specific device driver settings + + CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART + m (m=4,5) for the console and ttys0 (default is the USART1). + CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. + This specific the size of the receive buffer + CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before + being sent. This specific the size of the transmit buffer + CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be + CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. + CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity + CONFIG_U[S]ARTn_2STOP - Two stop bits + + CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI + support. Non-interrupt-driven, poll-waiting is recommended if the + interrupt rate would be to high in the interrupt driven case. + CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. + Cannot be used with CONFIG_STM32_SPI_INTERRUPT. + + CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO + and CONFIG_STM32_DMA2. + CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 + CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. + Default: Medium + CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: + 4-bit transfer mode. + + STM3210E-EVAL CAN Configuration + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default + Standard 11-bit IDs. + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 + CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + + STM3210E-EVAL LCD Hardware Configuration + + CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" + support. Default is this 320x240 "landscape" orientation + (this setting is informative only... not used). + CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" + orientation support. In this orientation, the STM3210E-EVAL's + LCD ribbon cable is at the bottom of the display. Default is + 320x240 "landscape" orientation. + CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse + portrait" orientation support. In this orientation, the + STM3210E-EVAL's LCD ribbon cable is at the top of the display. + Default is 320x240 "landscape" orientation. + CONFIG_LCD_BACKLIGHT - Define to support a backlight. + CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an + adjustable backlight will be provided using timer 1 to generate + various pulse widthes. The granularity of the settings is + determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or + CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight + is provided. + CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears + to be a shift in the returned data. This value fixes the offset. + Default 5. + + The LCD driver dynamically selects the LCD based on the reported LCD + ID value. However, code size can be reduced by suppressing support for + individual LCDs using: + + CONFIG_STM32_AM240320_DISABLE + CONFIG_STM32_SPFD5408B_DISABLE + CONFIG_STM32_R61580_DISABLE + +Configurations +============== + +Each STM3210E-EVAL configuration is maintained in a sudirectory and +can be selected as follow: + + cd tools + ./configure.sh stm3210e-eval/ + cd - + . ./setenv.sh + +Where is one of the following: + + buttons: + -------- + + Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and + button interrupts. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + composite + --------- + + This configuration exercises a composite USB interface consisting + of a CDC/ACM device and a USB mass storage device. This configuration + uses apps/examples/composite. + + nsh and nsh2: + ------------ + Configure the NuttShell (nsh) located at examples/nsh. + + Differences between the two NSH configurations: + + =========== ======================= ================================ + nsh nsh2 + =========== ======================= ================================ + Toolchain: NuttX buildroot for Codesourcery for Windows (1) + Linux or Cygwin (1,2) + ----------- ----------------------- -------------------------------- + Loader: DfuSe DfuSe + ----------- ----------------------- -------------------------------- + Serial Debug output: USART1 Debug output: USART1 + Console: NSH output: USART1 NSH output: USART1 (3) + ----------- ----------------------- -------------------------------- + microSD Yes Yes + Support + ----------- ----------------------- -------------------------------- + FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y + Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) + ----------- ----------------------- -------------------------------- + Support for No Yes + Built-in + Apps + ----------- ----------------------- -------------------------------- + Built-in None apps/examples/nx + Apps apps/examples/nxhello + apps/examples/usbstorage (5) + =========== ======================= ================================ + + (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh + to set up the correct PATH variable for whichever toolchain you + may use. + (2) Since DfuSe is assumed, this configuration may only work under + Cygwin without modification. + (3) When any other device other than /dev/console is used for a user + interface, (1) linefeeds (\n) will not be expanded to carriage return + / linefeeds \r\n). You will need to configure your terminal program + to account for this. And (2) input is not automatically echoed so + you will have to turn local echo on. + (4) Microsoft holds several patents related to the design of + long file names in the FAT file system. Please refer to the + details in the top-level COPYING file. Please do not use FAT + long file name unless you are familiar with these patent issues. + (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), + Caution should be used to assure that the SD drive is not in use when + the USB storage device is configured. Specifically, the SD driver + should be unmounted like: + + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH + ... + nsh> umount /mnd/sdcard # Unmount before connecting USB!!! + nsh> msconn # Connect the USB storage device + ... + nsh> msdis # Disconnect USB storate device + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount + + Failure to do this could result in corruption of the SD card format. + + nx: + --- + An example using the NuttX graphics system (NX). This example + focuses on general window controls, movement, mouse and keyboard + input. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxlines: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing lines on the background in various + orientations. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxtext: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing text on the background while pop-up + windows occur. Text should continue to update normally with + or without the popup windows present. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + NOTE: When I tried building this example with the CodeSourcery + tools, I got a hardfault inside of its libgcc. I haven't + retested since then, but beware if you choose to change the + toolchain. + + ostest: + ------ + This configuration directory, performs a simple OS test using + examples/ostest. By default, this project assumes that you are + using the DFU bootloader. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + RIDE + ---- + This configuration builds a trivial bring-up binary. It is + useful only because it words with the RIDE7 IDE and R-Link debugger. + + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + + usbserial: + --------- + This configuration directory exercises the USB serial class + driver at examples/usbserial. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + USB debug output can be enabled as by changing the following + settings in the configuration file: + + -CONFIG_DEBUG=n + -CONFIG_DEBUG_VERBOSE=n + -CONFIG_DEBUG_USB=n + +CONFIG_DEBUG=y + +CONFIG_DEBUG_VERBOSE=y + +CONFIG_DEBUG_USB=y + + -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n + -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n + +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y + +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y + + By default, the usbserial example uses the Prolific PL2303 + serial/USB converter emulation. The example can be modified + to use the CDC/ACM serial class by making the following changes + to the configuration file: + + -CONFIG_PL2303=y + +CONFIG_PL2303=n + + -CONFIG_CDCACM=n + +CONFIG_CDCACM=y + + The example can also be converted to use the alternative + USB serial example at apps/examples/usbterm by changing the + following: + + -CONFIGURED_APPS += examples/usbserial + +CONFIGURED_APPS += examples/usbterm + + In either the original appconfig file (before configuring) + or in the final apps/.config file (after configuring). + + usbstorage: + ---------- + This configuration directory exercises the USB mass storage + class driver at examples/usbstorage. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + diff --git a/nuttx/configs/px4iov2/common/Make.defs b/nuttx/configs/px4iov2/common/Make.defs new file mode 100644 index 000000000..7f782b5b2 --- /dev/null +++ b/nuttx/configs/px4iov2/common/Make.defs @@ -0,0 +1,175 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# enable precise stack overflow tracking +#INSTRUMENTATIONDEFINES = -finstrument-functions \ +# -ffixed-r10 + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx/configs/px4iov2/common/ld.script b/nuttx/configs/px4iov2/common/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx/configs/px4iov2/common/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx/configs/px4iov2/common/setenv.sh b/nuttx/configs/px4iov2/common/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx/configs/px4iov2/common/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/include/README.txt b/nuttx/configs/px4iov2/include/README.txt new file mode 100755 index 000000000..2264a80aa --- /dev/null +++ b/nuttx/configs/px4iov2/include/README.txt @@ -0,0 +1 @@ +This directory contains header files unique to the PX4IO board. diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx/configs/px4iov2/include/board.h new file mode 100755 index 000000000..b8dc71144 --- /dev/null +++ b/nuttx/configs/px4iov2/include/board.h @@ -0,0 +1,173 @@ +/************************************************************************************ + * configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * 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 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +# include +#endif +#include +#include +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 24000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + +#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#define GPIO_USART2_CTS 0xffffffff +#define GPIO_USART2_RTS 0xffffffff +#define GPIO_USART2_CK 0xffffffff +#define GPIO_USART3_TX 0xffffffff +#define GPIO_USART3_CK 0xffffffff +#define GPIO_USART3_CTS 0xffffffff +#define GPIO_USART3_RTS 0xffffffff + +/* + * High-resolution timer + */ +#ifdef CONFIG_HRT_TIMER +# define HRT_TIMER 1 /* use timer1 for the HRT */ +# define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#endif + +/* + * PPM + * + * PPM input is handled by the HRT timer. + * + * Pin is PA8, timer 1, channel 1 + */ +#if defined(CONFIG_HRT_TIMER) && defined (CONFIG_HRT_PPM) +# define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +# define GPIO_PPM_IN GPIO_TIM1_CH1IN +#endif + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4iov2/include/drv_i2c_device.h b/nuttx/configs/px4iov2/include/drv_i2c_device.h new file mode 100644 index 000000000..02582bc09 --- /dev/null +++ b/nuttx/configs/px4iov2/include/drv_i2c_device.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); +extern bool i2c_fsm(void); diff --git a/nuttx/configs/px4iov2/io/Make.defs b/nuttx/configs/px4iov2/io/Make.defs new file mode 100644 index 000000000..369772d03 --- /dev/null +++ b/nuttx/configs/px4iov2/io/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4io/common/Make.defs diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx/configs/px4iov2/io/appconfig new file mode 100644 index 000000000..628607a51 --- /dev/null +++ b/nuttx/configs/px4iov2/io/appconfig @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +CONFIGURED_APPS += drivers/boards/px4io +CONFIGURED_APPS += drivers/stm32 + +CONFIGURED_APPS += px4io + +# Mixer library from systemlib +CONFIGURED_APPS += systemlib/mixer diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig new file mode 100755 index 000000000..bb937cf4e --- /dev/null +++ b/nuttx/configs/px4iov2/io/defconfig @@ -0,0 +1,548 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD="px4io" +CONFIG_ARCH_BOARD_PX4IO=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4iov2/io/setenv.sh b/nuttx/configs/px4iov2/io/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx/configs/px4iov2/io/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/nsh/Make.defs b/nuttx/configs/px4iov2/nsh/Make.defs new file mode 100644 index 000000000..87508e22e --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs diff --git a/nuttx/configs/px4iov2/nsh/appconfig b/nuttx/configs/px4iov2/nsh/appconfig new file mode 100644 index 000000000..3cfc41b43 --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/appconfig @@ -0,0 +1,44 @@ +############################################################################ +# configs/stm3210e-eval/nsh/appconfig +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +CONFIGURED_APPS += system/readline +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += reboot + +CONFIGURED_APPS += drivers/boards/px4iov2 diff --git a/nuttx/configs/px4iov2/nsh/defconfig b/nuttx/configs/px4iov2/nsh/defconfig new file mode 100755 index 000000000..14d7a6401 --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/defconfig @@ -0,0 +1,567 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip familyl. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_DRAM_END - Last address+1 of installed RAM +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH=arm +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP=stm32 +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD=px4iov2 +CONFIG_ARCH_BOARD_PX4IOV2=y +CONFIG_BOARD_LOOPSPERMSEC=24000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=y +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=n +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# AHB: +CONFIG_STM32_DMA1=n +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + +# +# Timer and I2C devices may need to the following to force power to be applied: +# +#CONFIG_STM32_FORCEPOWER=y + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=57600 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y +CONFIG_PWM_SERVO=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_NXFLAT. Enable support for the NXFLAT binary format. +# This format will support execution of NuttX binaries located +# in a ROMFS filesystem (see examples/nxflat). +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=n +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=0 +CONFIG_START_YEAR=2009 +CONFIG_START_MONTH=9 +CONFIG_START_DAY=21 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NXFLAT=n +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=(50*1000) +CONFIG_SCHED_WORKSTACKSIZE=512 +CONFIG_SIG_SIGWORK=4 + +CONFIG_USER_ENTRYPOINT="nsh_main" +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=6 +CONFIG_NFILE_STREAMS=4 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=1 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=3 +CONFIG_PREALLOC_TIMERS=1 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_BUILTIN=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=64 +CONFIG_NSH_STRERROR=n +CONFIG_NSH_LINELEN=64 +CONFIG_NSH_NESTDEPTH=1 +CONFIG_NSH_DISABLESCRIPT=y +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=n +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=n +CONFIG_NSH_IOBUFFER_SIZE=256 +CONFIG_NSH_STACKSIZE=1024 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=n +CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) +CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) +CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0) +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=64 +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +CONFIG_NSH_MMCSDSPIPORTNO=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=800 +CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_PTHREAD_STACK_MIN=256 +CONFIG_PTHREAD_STACK_DEFAULT=512 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4iov2/nsh/setenv.sh b/nuttx/configs/px4iov2/nsh/setenv.sh new file mode 100755 index 000000000..d83685192 --- /dev/null +++ b/nuttx/configs/px4iov2/nsh/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/src/Makefile b/nuttx/configs/px4iov2/src/Makefile new file mode 100644 index 000000000..bb9539d16 --- /dev/null +++ b/nuttx/configs/px4iov2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/stm3210e-eval/src/Makefile +# +# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx/configs/px4iov2/src/README.txt b/nuttx/configs/px4iov2/src/README.txt new file mode 100644 index 000000000..d4eda82fd --- /dev/null +++ b/nuttx/configs/px4iov2/src/README.txt @@ -0,0 +1 @@ +This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx/configs/px4iov2/src/drv_i2c_device.c b/nuttx/configs/px4iov2/src/drv_i2c_device.c new file mode 100644 index 000000000..1f5931ae5 --- /dev/null +++ b/nuttx/configs/px4iov2/src/drv_i2c_device.c @@ -0,0 +1,618 @@ +/**************************************************************************** + * + * 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 A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +#include + +#include "stm32_i2c.h" + +#include + +/* + * I2C register definitions. + */ +#define I2C_BASE STM32_I2C1_BASE + +#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) + +#define rCR1 REG(STM32_I2C_CR1_OFFSET) +#define rCR2 REG(STM32_I2C_CR2_OFFSET) +#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) +#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) +#define rDR REG(STM32_I2C_DR_OFFSET) +#define rSR1 REG(STM32_I2C_SR1_OFFSET) +#define rSR2 REG(STM32_I2C_SR2_OFFSET) +#define rCCR REG(STM32_I2C_CCR_OFFSET) +#define rTRISE REG(STM32_I2C_TRISE_OFFSET) + +/* + * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib + */ +#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ +#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ +#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ +#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ +#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ + +/** + * States implemented by the I2C FSM. + */ +enum fsm_state { + BAD_PHASE, // must be zero, default exit on a bad state transition + + WAIT_FOR_MASTER, + + /* write from master */ + WAIT_FOR_COMMAND, + RECEIVE_COMMAND, + RECEIVE_DATA, + HANDLE_COMMAND, + + /* read from master */ + WAIT_TO_SEND, + SEND_STATUS, + SEND_DATA, + + NUM_STATES +}; + +/** + * Events recognised by the I2C FSM. + */ +enum fsm_event { + /* automatic transition */ + AUTO, + + /* write from master */ + ADDRESSED_WRITE, + BYTE_RECEIVED, + STOP_RECEIVED, + + /* read from master */ + ADDRESSED_READ, + BYTE_SENDABLE, + ACK_FAILED, + + NUM_EVENTS +}; + +/** + * Context for the I2C FSM + */ +static struct fsm_context { + enum fsm_state state; + + /* XXX want to eliminate these */ + uint8_t command; + uint8_t status; + + uint8_t *data_ptr; + uint32_t data_count; + + size_t buffer_size; + uint8_t *buffer; +} context; + +/** + * Structure defining one FSM state and its outgoing transitions. + */ +struct fsm_transition { + void (*handler)(void); + enum fsm_state next_state[NUM_EVENTS]; +}; + +static bool i2c_command_received; + +static void fsm_event(enum fsm_event event); + +static void go_bad(void); +static void go_wait_master(void); + +static void go_wait_command(void); +static void go_receive_command(void); +static void go_receive_data(void); +static void go_handle_command(void); + +static void go_wait_send(void); +static void go_send_status(void); +static void go_send_buffer(void); + +/** + * The FSM state graph. + */ +static const struct fsm_transition fsm[NUM_STATES] = { + [BAD_PHASE] = { + .handler = go_bad, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + [WAIT_FOR_MASTER] = { + .handler = go_wait_master, + .next_state = { + [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, + [ADDRESSED_READ] = WAIT_TO_SEND, + }, + }, + + /* write from master*/ + [WAIT_FOR_COMMAND] = { + .handler = go_wait_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_COMMAND, + [STOP_RECEIVED] = WAIT_FOR_MASTER, + }, + }, + [RECEIVE_COMMAND] = { + .handler = go_receive_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [RECEIVE_DATA] = { + .handler = go_receive_data, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [HANDLE_COMMAND] = { + .handler = go_handle_command, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + /* buffer send */ + [WAIT_TO_SEND] = { + .handler = go_wait_send, + .next_state = { + [BYTE_SENDABLE] = SEND_STATUS, + }, + }, + [SEND_STATUS] = { + .handler = go_send_status, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, + [SEND_DATA] = { + .handler = go_send_buffer, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, +}; + + +/* debug support */ +#if 1 +struct fsm_logentry { + char kind; + uint32_t code; +}; + +#define LOG_ENTRIES 32 +static struct fsm_logentry fsm_log[LOG_ENTRIES]; +int fsm_logptr; +#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) +#define LOGx(_kind, _code) \ + do { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr = LOG_NEXT(fsm_logptr); \ + fsm_log[fsm_logptr].kind = 0; \ + } while(0) + +#define LOG(_kind, _code) \ + do {\ + if (fsm_logptr < LOG_ENTRIES) { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr++;\ + }\ + }while(0) + +#else +#define LOG(_kind, _code) +#endif + + +static void i2c_setclock(uint32_t frequency); + +/** + * Initialise I2C + * + */ +void +i2c_fsm_init(uint8_t *buffer, size_t buffer_size) +{ + /* save the buffer */ + context.buffer = buffer; + context.buffer_size = buffer_size; + + // initialise the FSM + context.status = 0; + context.command = 0; + context.state = BAD_PHASE; + fsm_event(AUTO); + +#if 0 + // enable the i2c block clock and reset it + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + + // configure the i2c GPIOs + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + + // set the peripheral clock to match the APB clock + rCR2 = STM32_PCLK1_FREQUENCY / 1000000; + + // configure for 100kHz operation + i2c_setclock(100000); + + // enable i2c + rCR1 = I2C_CR1_PE; +#endif +} + +/** + * Run the I2C FSM for some period. + * + * @return True if the buffer has been updated by a command. + */ +bool +i2c_fsm(void) +{ + uint32_t event; + int idle_iterations = 0; + + for (;;) { + // handle bus error states by discarding the current operation + if (rSR1 & I2C_SR1_BERR) { + context.state = WAIT_FOR_MASTER; + rSR1 = ~I2C_SR1_BERR; + } + + // we do not anticipate over/underrun errors as clock-stretching is enabled + + // fetch the most recent event + event = ((rSR2 << 16) | rSR1) & 0x00ffffff; + + // generate FSM events based on I2C events + switch (event) { + case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: + LOG('w', 0); + fsm_event(ADDRESSED_WRITE); + break; + + case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: + LOG('r', 0); + fsm_event(ADDRESSED_READ); + break; + + case I2C_EVENT_SLAVE_BYTE_RECEIVED: + LOG('R', 0); + fsm_event(BYTE_RECEIVED); + break; + + case I2C_EVENT_SLAVE_STOP_DETECTED: + LOG('s', 0); + fsm_event(STOP_RECEIVED); + break; + + case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: + //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: + LOG('T', 0); + fsm_event(BYTE_SENDABLE); + break; + + case I2C_EVENT_SLAVE_ACK_FAILURE: + LOG('a', 0); + fsm_event(ACK_FAILED); + break; + + default: + idle_iterations++; +// if ((event) && (event != 0x00020000)) +// LOG('e', event); + break; + } + + /* if we have just received something, drop out and let the caller handle it */ + if (i2c_command_received) { + i2c_command_received = false; + return true; + } + + /* if we have done nothing recently, drop out and let the caller have a slice */ + if (idle_iterations > 1000) + return false; + } +} + +/** + * Update the FSM with an event + * + * @param event New event. + */ +static void +fsm_event(enum fsm_event event) +{ + // move to the next state + context.state = fsm[context.state].next_state[event]; + + LOG('f', context.state); + + // call the state entry handler + if (fsm[context.state].handler) { + fsm[context.state].handler(); + } +} + +static void +go_bad() +{ + LOG('B', 0); + fsm_event(AUTO); +} + +/** + * Wait for the master to address us. + * + */ +static void +go_wait_master() +{ + // We currently don't have a command byte. + // + context.command = '\0'; + + // The data pointer starts pointing to the start of the data buffer. + // + context.data_ptr = context.buffer; + + // The data count is either: + // - the size of the data buffer + // - some value less than or equal the size of the data buffer during a write or a read + // + context.data_count = context.buffer_size; + + // (re)enable the peripheral, clear the stop event flag in + // case we just finished receiving data + rCR1 |= I2C_CR1_PE; + + // clear the ACK failed flag in case we just finished sending data + rSR1 = ~I2C_SR1_AF; +} + +/** + * Prepare to receive a command byte. + * + */ +static void +go_wait_command() +{ + // NOP +} + +/** + * Command byte has been received, save it and prepare to handle the data. + * + */ +static void +go_receive_command() +{ + + // fetch the command byte + context.command = (uint8_t)rDR; + LOG('c', context.command); + +} + +/** + * Receive a data byte. + * + */ +static void +go_receive_data() +{ + uint8_t d; + + // fetch the byte + d = (uint8_t)rDR; + LOG('d', d); + + // if we have somewhere to put it, do so + if (context.data_count) { + *context.data_ptr++ = d; + context.data_count--; + } +} + +/** + * Handle a command once the host is done sending it to us. + * + */ +static void +go_handle_command() +{ + // presume we are happy with the command + context.status = 0; + + // make a note that the buffer contains a fresh command + i2c_command_received = true; + + // kick along to the next state + fsm_event(AUTO); +} + +/** + * Wait to be able to send the status byte. + * + */ +static void +go_wait_send() +{ + // NOP +} + +/** + * Send the status byte. + * + */ +static void +go_send_status() +{ + rDR = context.status; + LOG('?', context.status); +} + +/** + * Send a data or pad byte. + * + */ +static void +go_send_buffer() +{ + if (context.data_count) { + LOG('D', *context.data_ptr); + rDR = *(context.data_ptr++); + context.data_count--; + } else { + LOG('-', 0); + rDR = 0xff; + } +} + +/* cribbed directly from the NuttX master driver */ +static void +i2c_setclock(uint32_t frequency) +{ + uint16_t cr1; + uint16_t ccr; + uint16_t trise; + uint16_t freqmhz; + uint16_t speed; + + /* Disable the selected I2C peripheral to configure TRISE */ + + cr1 = rCR1; + rCR1 &= ~I2C_CR1_PE; + + /* Update timing and control registers */ + + freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); + ccr = 0; + + /* Configure speed in standard mode */ + + if (frequency <= 100000) { + /* Standard mode speed calculation */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); + + /* The CCR fault must be >= 4 */ + + if (speed < 4) { + /* Set the minimum allowed value */ + + speed = 4; + } + ccr |= speed; + + /* Set Maximum Rise Time for standard mode */ + + trise = freqmhz + 1; + + /* Configure speed in fast mode */ + } else { /* (frequency <= 400000) */ + /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ + +#ifdef CONFIG_I2C_DUTY16_9 + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); + + /* Set DUTY and fast speed bits */ + + ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); +#else + /* Fast mode speed calculation with Tlow/Thigh = 2 */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); + + /* Set fast speed bit */ + + ccr |= I2C_CCR_FS; +#endif + + /* Verify that the CCR speed value is nonzero */ + + if (speed < 1) { + /* Set the minimum allowed value */ + + speed = 1; + } + ccr |= speed; + + /* Set Maximum Rise Time for fast mode */ + + trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); + } + + /* Write the new values of the CCR and TRISE registers */ + + rCCR = ccr; + rTRISE = trise; + + /* Bit 14 of OAR1 must be configured and kept at 1 */ + + rOAR1 = I2C_OAR1_ONE); + + /* Re-enable the peripheral (or not) */ + + rCR1 = cr1; +} diff --git a/nuttx/configs/px4iov2/src/empty.c b/nuttx/configs/px4iov2/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx/configs/px4iov2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ -- cgit v1.2.3 From 31c6440b6491cd6310c9db72be200a4ffba689c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Apr 2013 11:54:24 -0700 Subject: Un-ignore the *.d directories in the ROMFS directory to avoid ignoring the init.d directory and friends. This is rinky, but the alternatives are all a mess. --- .gitignore | 3 + ROMFS/px4fmu_common/init.d/rc.FMU_quad_x | 67 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.PX4IO | 80 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.PX4IOAR | 98 +++++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.boarddetect | 66 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.jig | 10 ++++ ROMFS/px4fmu_common/init.d/rc.logging | 9 +++ ROMFS/px4fmu_common/init.d/rc.sensors | 34 +++++++++++ ROMFS/px4fmu_common/init.d/rc.standalone | 13 ++++ ROMFS/px4fmu_common/init.d/rcS | 79 +++++++++++++++++++++++++ 10 files changed, 459 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.FMU_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/rc.PX4IO create mode 100644 ROMFS/px4fmu_common/init.d/rc.PX4IOAR create mode 100644 ROMFS/px4fmu_common/init.d/rc.boarddetect create mode 100644 ROMFS/px4fmu_common/init.d/rc.jig create mode 100644 ROMFS/px4fmu_common/init.d/rc.logging create mode 100644 ROMFS/px4fmu_common/init.d/rc.sensors create mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone create mode 100755 ROMFS/px4fmu_common/init.d/rcS diff --git a/.gitignore b/.gitignore index 46b347f72..91358e1e1 100644 --- a/.gitignore +++ b/.gitignore @@ -56,3 +56,6 @@ core mkdeps Archives Build +!ROMFS/*/*.d +!ROMFS/*/*/*.d +!ROMFS/*/*/*/*.d diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x new file mode 100644 index 000000000..8787443ea --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x @@ -0,0 +1,67 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE custom + +echo "[init] doing PX4FMU Quad startup..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Start attitude control +# +multirotor_att_control start + +echo "[init] startup done, exiting" +exit \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.PX4IO new file mode 100644 index 000000000..1e3963b9a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IO @@ -0,0 +1,80 @@ +#!nsh + +# Disable USB and autostart +set USB no +set MODE camflyer + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start GPS interface +# +gps start + +# +# Start the attitude estimator +# +kalman_demo start + +# +# Start PX4IO interface +# +px4io start + +# +# Load mixer and start controllers +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog start -s 10 + +# +# Start system state +# +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR new file mode 100644 index 000000000..72df68e35 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR @@ -0,0 +1,98 @@ +#!nsh +# +# Flight startup script for PX4FMU on PX4IOAR carrier board. +# + +# Disable the USB interface +set USB no + +# Disable autostarting other apps +set MODE ardrone + +echo "[init] doing PX4IOAR startup..." + +# +# Start the ORB +# +uorb start + +# +# Init the parameter storage +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start the sensors. +# +sh /etc/init.d/rc.sensors + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the commander. +# +commander start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Configure PX4FMU for operation with PX4IOAR +# +fmu mode_gpio_serial + +# +# Fire up the multi rotor attitude controller +# +multirotor_att_control start + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 + +# +# Start GPS capture +# +gps start + +# +# Start logging +# +sdlog start -s 10 + +# +# Start system state +# +if blinkm start +then + echo "using BlinkM for state indication" + blinkm systemstate +else + echo "no BlinkM found, OK." +fi + +# +# startup is done; we don't want the shell because we +# use the same UART for telemetry +# +echo "[init] startup done" +exit \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect new file mode 100644 index 000000000..f233e51df --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boarddetect @@ -0,0 +1,66 @@ +#!nsh +# +# If we are still in flight mode, work out what airframe +# configuration we have and start up accordingly. +# +if [ $MODE != autostart ] +then + echo "[init] automatic startup cancelled by user script" +else + echo "[init] detecting attached hardware..." + + # + # Assume that we are PX4FMU in standalone mode + # + set BOARD PX4FMU + + # + # Are we attached to a PX4IOAR (AR.Drone carrier board)? + # + if boardinfo test name PX4IOAR + then + set BOARD PX4IOAR + if [ -f /etc/init.d/rc.PX4IOAR ] + then + echo "[init] reading /etc/init.d/rc.PX4IOAR" + usleep 500 + sh /etc/init.d/rc.PX4IOAR + fi + else + echo "[init] PX4IOAR not detected" + fi + + # + # Are we attached to a PX4IO? + # + if boardinfo test name PX4IO + then + set BOARD PX4IO + if [ -f /etc/init.d/rc.PX4IO ] + then + echo "[init] reading /etc/init.d/rc.PX4IO" + usleep 500 + sh /etc/init.d/rc.PX4IO + fi + else + echo "[init] PX4IO not detected" + fi + + # + # Looks like we are stand-alone + # + if [ $BOARD == PX4FMU ] + then + echo "[init] no expansion board detected" + if [ -f /etc/init.d/rc.standalone ] + then + echo "[init] reading /etc/init.d/rc.standalone" + sh /etc/init.d/rc.standalone + fi + fi + + # + # We may not reach here if the airframe-specific script exits the shell. + # + echo "[init] startup done." +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig new file mode 100644 index 000000000..e2b5d8f30 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.jig @@ -0,0 +1,10 @@ +#!nsh +# +# Test jig startup script +# + +echo "[testing] doing production test.." + +tests jig + +echo "[testing] testing done" diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging new file mode 100644 index 000000000..09c2d00d1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -0,0 +1,9 @@ +#!nsh +# +# Initialise logging services. +# + +if [ -d /fs/microsd ] +then + sdlog start +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors new file mode 100644 index 000000000..42c2f52e9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -0,0 +1,34 @@ +#!nsh +# +# Standard startup script for PX4FMU onboard sensor drivers. +# + +# +# Start sensor drivers here. +# + +ms5611 start +adc start + +if mpu6000 start +then + echo "using MPU6000 and HMC5883L" + hmc5883 start +else + echo "using L3GD20 and LSM303D" + l3gd20 start + lsm303 start +fi + +# +# Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. +# +sensors start + +# +# Check sensors - run AFTER 'sensors start' +# +preflight_check \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone new file mode 100644 index 000000000..67e95215b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.standalone @@ -0,0 +1,13 @@ +#!nsh +# +# Flight startup script for PX4FMU standalone configuration. +# + +echo "[init] doing standalone PX4FMU startup..." + +# +# Start the ORB +# +uorb start + +echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS new file mode 100755 index 000000000..89a767879 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -0,0 +1,79 @@ +#!nsh +# +# PX4FMU startup script. +# +# This script is responsible for: +# +# - mounting the microSD card (if present) +# - running the user startup script from the microSD card (if present) +# - detecting the configuration of the system and picking a suitable +# startup script to continue with +# +# Note: DO NOT add configuration-specific commands to this script; +# add them to the per-configuration scripts instead. +# + +# +# Default to auto-start mode. An init script on the microSD card +# can change this to prevent automatic startup of the flight script. +# +set MODE autostart +set USB autoconnect + +# +# Start playing the startup tune +# +tone_alarm start + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" +else + echo "[init] no microSD card found" +fi + +# +# Look for an init script on the microSD card. +# +# To prevent automatic startup in the current flight mode, +# the script should set MODE to some other value. +# +if [ -f /fs/microsd/etc/rc ] +then + echo "[init] reading /fs/microsd/etc/rc" + sh /fs/microsd/etc/rc +fi +# Also consider rc.txt files +if [ -f /fs/microsd/etc/rc.txt ] +then + echo "[init] reading /fs/microsd/etc/rc.txt" + sh /fs/microsd/etc/rc.txt +fi + +# +# Check for USB host +# +if [ $USB != autoconnect ] +then + echo "[init] not connecting USB" +else + if sercon + then + echo "[init] USB interface connected" + else + echo "[init] No USB connected" + fi +fi + +# if this is an APM build then there will be a rc.APM script +# from an EXTERNAL_SCRIPTS build option +if [ -f /etc/init.d/rc.APM ] +then + echo Running rc.APM + # if APM startup is successful then nsh will exit + sh /etc/init.d/rc.APM +fi -- cgit v1.2.3 From 0eddcb335707284269cf9c89ac6b820efa1a6b13 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 10:56:58 -0700 Subject: Tried to collect some changes that I needed to build for FMUv2 into a commit --- makefiles/config_px4fmu_default.mk | 5 +- nuttx/configs/px4fmu/include/board.h | 6 + nuttx/configs/px4fmu/nsh/appconfig | 5 +- nuttx/configs/px4fmu/nsh/defconfig | 2 +- src/drivers/boards/px4fmu/module.mk | 9 + src/drivers/boards/px4fmu/px4fmu_can.c | 144 +++++++++++++++ src/drivers/boards/px4fmu/px4fmu_init.c | 266 +++++++++++++++++++++++++++ src/drivers/boards/px4fmu/px4fmu_internal.h | 162 ++++++++++++++++ src/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 87 +++++++++ src/drivers/boards/px4fmu/px4fmu_spi.c | 154 ++++++++++++++++ src/drivers/boards/px4fmu/px4fmu_usb.c | 108 +++++++++++ 11 files changed, 942 insertions(+), 6 deletions(-) create mode 100644 src/drivers/boards/px4fmu/module.mk create mode 100644 src/drivers/boards/px4fmu/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_init.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_internal.h create mode 100644 src/drivers/boards/px4fmu/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu/px4fmu_usb.c diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1a6c91b26..81dd202e7 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -10,7 +10,9 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -MODULES += drivers/px4fmu +MODULES += drivers/boards/px4fmu +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 # # Transitional support - add commands from the NuttX export archive. @@ -44,7 +46,6 @@ BUILTIN_COMMANDS := \ $(call _B, hmc5883, , 4096, hmc5883_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ - $(call _B, l3gd20, , 2048, l3gd20_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 0bc0b3021..294b6c398 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -308,6 +308,10 @@ #define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 #define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_2 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + #define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 #define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 #define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 @@ -321,6 +325,8 @@ #define PX4_SPIDEV_ACCEL 2 #define PX4_SPIDEV_MPU 3 +#define PX4_SPIDEV_ACCEL_MAG 2 // external for anti vibration test + /* * Tone alarm output */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index d642b4692..b60bbfdd9 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -110,16 +110,15 @@ endif CONFIGURED_APPS += systemcmds/i2c # Communication and Drivers -CONFIGURED_APPS += drivers/boards/px4fmu +#CONFIGURED_APPS += drivers/boards/px4fmu CONFIGURED_APPS += drivers/device CONFIGURED_APPS += drivers/ms5611 CONFIGURED_APPS += drivers/hmc5883 CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += drivers/bma180 -CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 -CONFIGURED_APPS += drivers/led +#CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 130886aac..cf30b835f 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -85,7 +85,7 @@ CONFIG_ARCH_FPU=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=y +CONFIG_ARCH_LEDS=n CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n CONFIG_ARCH_DMA=y diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk new file mode 100644 index 000000000..2cb261d30 --- /dev/null +++ b/src/drivers/boards/px4fmu/module.mk @@ -0,0 +1,9 @@ +# +# Board-specific startup code for the PX4FMU +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c new file mode 100644 index 000000000..86ba183b8 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "px4fmu_internal.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c new file mode 100644 index 000000000..96c7fa2df --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32_internal.h" +#include "px4fmu_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct spi_dev_s *spi3; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + int result; + +// /* configure ADC pins */ +// stm32_configgpio(GPIO_ADC1_IN10); +// stm32_configgpio(GPIO_ADC1_IN11); +// stm32_configgpio(GPIO_ADC1_IN12); + +// /* configure power supply control pins */ +// stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); +// stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); +// stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); +// stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + // initial LED state +// drv_led_start(); + up_ledoff(LED_BLUE); + up_ledoff(LED_AMBER); + up_ledon(LED_BLUE); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); +// SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); +// SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); +// SPI_SELECT(spi2, PX4_SPIDEV_ACCEL, false); +// SPI_SELECT(spi2, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port2\r\n"); + + /* Get the SPI port for the microSD slot */ + + message("[boot] Initializing SPI port 3\n"); + spi3 = up_spiinitialize(3); + + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 3\n"); + + /* Now bind the SPI interface to the MMCSD driver */ + result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); + + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); +// stm32_configgpio(GPIO_ADC1_IN12); +// stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + + return OK; +} diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h new file mode 100644 index 000000000..671a58f8f --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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 px4fmu_internal.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include + + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +//#ifdef CONFIG_STM32_SPI2 +//# error "SPI2 is not supported on this board" +//#endif + +#if defined(CONFIG_STM32_CAN1) +# warning "CAN1 is not supported on this board" +#endif + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + +/* External interrupts */ +#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +// XXX MPU6000 DRDY? + +/* SPI chip selects */ + +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + +/* User GPIOs + * + * GPIO0-1 are the buffered high-power GPIOs. + * GPIO2-5 are the USART2 pins. + * GPIO6-7 are the CAN2 pins. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) +#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* PWM + * + * The PX4FMU has five PWM outputs, of which only the output on + * pin PC8 is fixed assigned to this function. The other four possible + * pwm sources are the TX, RX, RTS and CTS pins of USART2 + * + * Alternate function mapping: + * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 + */ + +#ifdef CONFIG_PWM +# if defined(CONFIG_STM32_TIM3_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 3 +# elif defined(CONFIG_STM32_TIM8_PWM) +# define BUZZER_PWMCHANNEL 3 +# define BUZZER_PWMTIMER 8 +# endif +#endif + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c new file mode 100644 index 000000000..cb8918306 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c new file mode 100644 index 000000000..b5d00eac0 --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_spi.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL); + stm32_configgpio(GPIO_SPI_CS_MPU); + stm32_configgpio(GPIO_SPI_CS_SDCARD); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + break; + + case PX4_SPIDEV_ACCEL: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* this is actually bogus, but PX4 has no way to sense the presence of an SD card */ + return SPI_STATUS_PRESENT; +} + diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c new file mode 100644 index 000000000..b0b669fbe --- /dev/null +++ b/src/drivers/boards/px4fmu/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + -- cgit v1.2.3 From 76497502cb696a1c6b9b01ed8ef5c3a5a740cb52 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 11:20:31 -0700 Subject: Moved the L3GD20 driver to the new driver, working on FMU v1 and v2 --- apps/drivers/l3gd20/Makefile | 42 -- apps/drivers/l3gd20/l3gd20.cpp | 887 ----------------------------------- makefiles/config_px4fmuv2_default.mk | 2 +- nuttx/configs/px4fmuv2/nsh/appconfig | 2 - src/drivers/l3gd20/l3gd20.cpp | 887 +++++++++++++++++++++++++++++++++++ src/drivers/l3gd20/module.mk | 6 + 6 files changed, 894 insertions(+), 932 deletions(-) delete mode 100644 apps/drivers/l3gd20/Makefile delete mode 100644 apps/drivers/l3gd20/l3gd20.cpp create mode 100644 src/drivers/l3gd20/l3gd20.cpp create mode 100644 src/drivers/l3gd20/module.mk diff --git a/apps/drivers/l3gd20/Makefile b/apps/drivers/l3gd20/Makefile deleted file mode 100644 index 98e2d57c5..000000000 --- a/apps/drivers/l3gd20/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. -# -############################################################################ - -# -# Makefile to build the L3GD20 driver. -# - -APPNAME = l3gd20 -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp deleted file mode 100644 index c7f433dd4..000000000 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ /dev/null @@ -1,887 +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 Driver for the ST L3GD20 MEMS gyro connected via SPI. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include -#include - - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - -/* register addresses */ -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0xD4 - -#define ADDR_CTRL_REG1 0x20 -#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ -/* keep lowpass low to avoid noise issues */ -#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) - -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ -#define RANGE_250DPS (0<<4) -#define RANGE_500DPS (1<<4) -#define RANGE_2000DPS (3<<4) - -#define ADDR_CTRL_REG5 0x24 -#define ADDR_REFERENCE 0x25 -#define ADDR_OUT_TEMP 0x26 -#define ADDR_STATUS_REG 0x27 -#define ADDR_OUT_X_L 0x28 -#define ADDR_OUT_X_H 0x29 -#define ADDR_OUT_Y_L 0x2A -#define ADDR_OUT_Y_H 0x2B -#define ADDR_OUT_Z_L 0x2C -#define ADDR_OUT_Z_H 0x2D -#define ADDR_FIFO_CTRL_REG 0x2E -#define ADDR_FIFO_SRC_REG 0x2F -#define ADDR_INT1_CFG 0x30 -#define ADDR_INT1_SRC 0x31 -#define ADDR_INT1_TSH_XH 0x32 -#define ADDR_INT1_TSH_XL 0x33 -#define ADDR_INT1_TSH_YH 0x34 -#define ADDR_INT1_TSH_YL 0x35 -#define ADDR_INT1_TSH_ZH 0x36 -#define ADDR_INT1_TSH_ZL 0x37 -#define ADDR_INT1_DURATION 0x38 - - -/* Internal configuration values */ -#define REG1_POWER_NORMAL (1<<3) -#define REG1_Z_ENABLE (1<<2) -#define REG1_Y_ENABLE (1<<1) -#define REG1_X_ENABLE (1<<0) - -#define REG4_BDU (1<<7) -#define REG4_BLE (1<<6) -//#define REG4_SPI_3WIRE (1<<0) - -#define REG5_FIFO_ENABLE (1<<6) -#define REG5_REBOOT_MEMORY (1<<7) - -#define STATUS_ZYXOR (1<<7) -#define STATUS_ZOR (1<<6) -#define STATUS_YOR (1<<5) -#define STATUS_XOR (1<<4) -#define STATUS_ZYXDA (1<<3) -#define STATUS_ZDA (1<<2) -#define STATUS_YDA (1<<1) -#define STATUS_XDA (1<<0) - -#define FIFO_CTRL_BYPASS_MODE (0<<5) -#define FIFO_CTRL_FIFO_MODE (1<<5) -#define FIFO_CTRL_STREAM_MODE (1<<6) -#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) -#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) - -extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } - -class L3GD20 : public device::SPI -{ -public: - L3GD20(int bus, const char* path, spi_dev_e device); - virtual ~L3GD20(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - - struct hrt_call _call; - unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; - - struct gyro_scale _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - - unsigned _current_rate; - unsigned _current_range; - - perf_counter_t _sample_perf; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Fetch measurements from the sensor and update the report ring. - */ - void measure(); - - /** - * Read a register from the L3GD20 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the L3GD20 - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the L3GD20 - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Set the L3GD20 measurement range. - * - * @param max_dps The measurement range is set to permit reading at least - * this rate in degrees per second. - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_range(unsigned max_dps); - - /** - * Set the L3GD20 internal sampling frequency. - * - * @param frequency The internal sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int set_samplerate(unsigned frequency); -}; - -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - - -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), - _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _gyro_range_scale(0.0f), - _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _current_rate(0), - _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) -{ - // enable debug() calls - _debug_enabled = true; - - // default scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; -} - -L3GD20::~L3GD20() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; - - /* delete the perf counter */ - perf_free(_sample_perf); -} - -int -L3GD20::init() -{ - int ret = ERROR; - - /* do SPI init (and probe) first */ - if (SPI::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); - - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ - write_reg(ADDR_CTRL_REG4, REG4_BDU); - write_reg(ADDR_CTRL_REG5, 0); - - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ - - set_range(500); /* default to 500dps */ - set_samplerate(0); /* max sample rate */ - - ret = OK; -out: - return ret; -} - -int -L3GD20::probe() -{ - /* read dummy value to void to clear SPI statemachine on sensor */ - (void)read_reg(ADDR_WHO_AM_I); - - /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) - return OK; - - return -EIO; -} - -ssize_t -L3GD20::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct gyro_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_call_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _oldest_report = _next_report = 0; - measure(); - - /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - return ret; -} - -int -L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_interval = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call.period = _call_interval = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_interval == 0) - return SENSOR_POLLRATE_MANUAL; - - return 1000000 / _call_interval; - - case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct gyro_report *buf = new struct gyro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; - - case GYROIOCSSAMPLERATE: - return set_samplerate(arg); - - case GYROIOCGSAMPLERATE: - return _current_rate; - - case GYROIOCSLOWPASS: - case GYROIOCGLOWPASS: - /* XXX not implemented due to wacky interaction with samplerate */ - return -EINVAL; - - case GYROIOCSSCALE: - /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); - return OK; - - case GYROIOCGSCALE: - /* copy scale out */ - memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); - return OK; - - case GYROIOCSRANGE: - return set_range(arg); - - case GYROIOCGRANGE: - return _current_range; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -uint8_t -L3GD20::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -L3GD20::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int -L3GD20::set_range(unsigned max_dps) -{ - uint8_t bits = REG4_BDU; - - if (max_dps == 0) - max_dps = 2000; - - if (max_dps <= 250) { - _current_range = 250; - bits |= RANGE_250DPS; - - } else if (max_dps <= 500) { - _current_range = 500; - bits |= RANGE_500DPS; - - } else if (max_dps <= 2000) { - _current_range = 2000; - bits |= RANGE_2000DPS; - - } else { - return -EINVAL; - } - - _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; - write_reg(ADDR_CTRL_REG4, bits); - - return OK; -} - -int -L3GD20::set_samplerate(unsigned frequency) -{ - uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; - - if (frequency == 0) - frequency = 760; - - if (frequency <= 95) { - _current_rate = 95; - bits |= RATE_95HZ_LP_25HZ; - - } else if (frequency <= 190) { - _current_rate = 190; - bits |= RATE_190HZ_LP_25HZ; - - } else if (frequency <= 380) { - _current_rate = 380; - bits |= RATE_380HZ_LP_30HZ; - - } else if (frequency <= 760) { - _current_rate = 760; - bits |= RATE_760HZ_LP_30HZ; - - } else { - return -EINVAL; - } - - write_reg(ADDR_CTRL_REG1, bits); - - return OK; -} - -void -L3GD20::start() -{ - /* make sure we are stopped first */ - stop(); - - /* reset the report ring */ - _oldest_report = _next_report = 0; - - /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); -} - -void -L3GD20::stop() -{ - hrt_cancel(&_call); -} - -void -L3GD20::measure_trampoline(void *arg) -{ - L3GD20 *dev = (L3GD20 *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -L3GD20::measure() -{ - /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t temp; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_report; -#pragma pack(pop) - - gyro_report *report = &_reports[_next_report]; - - /* start the performance counter */ - perf_begin(_sample_perf); - - /* fetch data from the sensor */ - raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - report->timestamp = hrt_absolute_time(); - - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); - report->z_raw = raw_report.z; - - report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report->scaling = _gyro_range_scale; - report->range_rad_s = _gyro_range_rad_s; - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); - - /* stop the perf counter */ - perf_end(_sample_perf); -} - -void -L3GD20::print_info() -{ - perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); -} - -/** - * Local functions in support of the shell command. - */ -namespace l3gd20 -{ - -L3GD20 *g_dev; - -void start(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int fd_gyro = -1; - struct gyro_report g_report; - ssize_t sz; - - /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); - - if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); - - /* reset to manual polling */ - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) - err(1, "reset to manual polling"); - - /* do a simple demand read */ - sz = read(fd_gyro, &g_report, sizeof(g_report)); - - if (sz != sizeof(g_report)) - err(1, "immediate gyro read failed"); - - warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); - warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); - warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); - warnx("gyro x: \t%d\traw", (int)g_report.x_raw); - warnx("gyro y: \t%d\traw", (int)g_report.y_raw); - warnx("gyro z: \t%d\traw", (int)g_report.z_raw); - warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); - - /* XXX add poll-rate tests here too */ - - reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running\n"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - - -} // namespace - -int -l3gd20_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - - */ - if (!strcmp(argv[1], "start")) - l3gd20::start(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - l3gd20::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - l3gd20::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info")) - l3gd20::info(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d296c5379..ee0c17bcb 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -12,6 +12,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # MODULES += drivers/boards/px4fmuv2 MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 MODULES += drivers/px4fmu MODULES += drivers/rgbled @@ -43,7 +44,6 @@ BUILTIN_COMMANDS := \ $(call _B, hil, , 2048, hil_main ) \ $(call _B, hott_telemetry, , 2048, hott_telemetry_main ) \ $(call _B, kalman_demo, SCHED_PRIORITY_MAX-30, 2048, kalman_demo_main ) \ - $(call _B, l3gd20, , 2048, l3gd20_main ) \ $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, mavlink, , 2048, mavlink_main ) \ $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fff140673..205b4448c 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -115,8 +115,6 @@ CONFIGURED_APPS += drivers/boards/px4fmuv2 CONFIGURED_APPS += drivers/device # XXX needs conversion to SPI #CONFIGURED_APPS += drivers/ms5611 -CONFIGURED_APPS += drivers/l3gd20 -CONFIGURED_APPS += drivers/lsm303d # XXX needs conversion to serial #CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp new file mode 100644 index 000000000..c7f433dd4 --- /dev/null +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -0,0 +1,887 @@ +/**************************************************************************** + * + * 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 Driver for the ST L3GD20 MEMS gyro connected via SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +/* register addresses */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0xD4 + +#define ADDR_CTRL_REG1 0x20 +#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ +/* keep lowpass low to avoid noise issues */ +#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) + +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ +#define RANGE_250DPS (0<<4) +#define RANGE_500DPS (1<<4) +#define RANGE_2000DPS (3<<4) + +#define ADDR_CTRL_REG5 0x24 +#define ADDR_REFERENCE 0x25 +#define ADDR_OUT_TEMP 0x26 +#define ADDR_STATUS_REG 0x27 +#define ADDR_OUT_X_L 0x28 +#define ADDR_OUT_X_H 0x29 +#define ADDR_OUT_Y_L 0x2A +#define ADDR_OUT_Y_H 0x2B +#define ADDR_OUT_Z_L 0x2C +#define ADDR_OUT_Z_H 0x2D +#define ADDR_FIFO_CTRL_REG 0x2E +#define ADDR_FIFO_SRC_REG 0x2F +#define ADDR_INT1_CFG 0x30 +#define ADDR_INT1_SRC 0x31 +#define ADDR_INT1_TSH_XH 0x32 +#define ADDR_INT1_TSH_XL 0x33 +#define ADDR_INT1_TSH_YH 0x34 +#define ADDR_INT1_TSH_YL 0x35 +#define ADDR_INT1_TSH_ZH 0x36 +#define ADDR_INT1_TSH_ZL 0x37 +#define ADDR_INT1_DURATION 0x38 + + +/* Internal configuration values */ +#define REG1_POWER_NORMAL (1<<3) +#define REG1_Z_ENABLE (1<<2) +#define REG1_Y_ENABLE (1<<1) +#define REG1_X_ENABLE (1<<0) + +#define REG4_BDU (1<<7) +#define REG4_BLE (1<<6) +//#define REG4_SPI_3WIRE (1<<0) + +#define REG5_FIFO_ENABLE (1<<6) +#define REG5_REBOOT_MEMORY (1<<7) + +#define STATUS_ZYXOR (1<<7) +#define STATUS_ZOR (1<<6) +#define STATUS_YOR (1<<5) +#define STATUS_XOR (1<<4) +#define STATUS_ZYXDA (1<<3) +#define STATUS_ZDA (1<<2) +#define STATUS_YDA (1<<1) +#define STATUS_XDA (1<<0) + +#define FIFO_CTRL_BYPASS_MODE (0<<5) +#define FIFO_CTRL_FIFO_MODE (1<<5) +#define FIFO_CTRL_STREAM_MODE (1<<6) +#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) +#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) + +extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } + +class L3GD20 : public device::SPI +{ +public: + L3GD20(int bus, const char* path, spi_dev_e device); + virtual ~L3GD20(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct gyro_report *_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + orb_advert_t _gyro_topic; + + unsigned _current_rate; + unsigned _current_range; + + perf_counter_t _sample_perf; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the L3GD20 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the L3GD20 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the L3GD20 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the L3GD20 measurement range. + * + * @param max_dps The measurement range is set to permit reading at least + * this rate in degrees per second. + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_dps); + + /** + * Set the L3GD20 internal sampling frequency. + * + * @param frequency The internal sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int set_samplerate(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + + +L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), + _call_interval(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _gyro_topic(-1), + _current_rate(0), + _current_range(0), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; +} + +L3GD20::~L3GD20() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; + + /* delete the perf counter */ + perf_free(_sample_perf); +} + +int +L3GD20::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _oldest_report = _next_report = 0; + _reports = new struct gyro_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + /* advertise sensor topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG4, REG4_BDU); + write_reg(ADDR_CTRL_REG5, 0); + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + set_range(500); /* default to 500dps */ + set_samplerate(0); /* max sample rate */ + + ret = OK; +out: + return ret; +} + +int +L3GD20::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +L3GD20::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct gyro_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + return ret; +} + +int +L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* With internal low pass filters enabled, 250 Hz is sufficient */ + return ioctl(filp, SENSORIOCSPOLLRATE, 250); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct gyro_report *buf = new struct gyro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + case GYROIOCSSAMPLERATE: + return set_samplerate(arg); + + case GYROIOCGSAMPLERATE: + return _current_rate; + + case GYROIOCSLOWPASS: + case GYROIOCGLOWPASS: + /* XXX not implemented due to wacky interaction with samplerate */ + return -EINVAL; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + return set_range(arg); + + case GYROIOCGRANGE: + return _current_range; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +L3GD20::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +L3GD20::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +L3GD20::set_range(unsigned max_dps) +{ + uint8_t bits = REG4_BDU; + + if (max_dps == 0) + max_dps = 2000; + + if (max_dps <= 250) { + _current_range = 250; + bits |= RANGE_250DPS; + + } else if (max_dps <= 500) { + _current_range = 500; + bits |= RANGE_500DPS; + + } else if (max_dps <= 2000) { + _current_range = 2000; + bits |= RANGE_2000DPS; + + } else { + return -EINVAL; + } + + _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; + _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + write_reg(ADDR_CTRL_REG4, bits); + + return OK; +} + +int +L3GD20::set_samplerate(unsigned frequency) +{ + uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; + + if (frequency == 0) + frequency = 760; + + if (frequency <= 95) { + _current_rate = 95; + bits |= RATE_95HZ_LP_25HZ; + + } else if (frequency <= 190) { + _current_rate = 190; + bits |= RATE_190HZ_LP_25HZ; + + } else if (frequency <= 380) { + _current_rate = 380; + bits |= RATE_380HZ_LP_30HZ; + + } else if (frequency <= 760) { + _current_rate = 760; + bits |= RATE_760HZ_LP_30HZ; + + } else { + return -EINVAL; + } + + write_reg(ADDR_CTRL_REG1, bits); + + return OK; +} + +void +L3GD20::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); +} + +void +L3GD20::stop() +{ + hrt_cancel(&_call); +} + +void +L3GD20::measure_trampoline(void *arg) +{ + L3GD20 *dev = (L3GD20 *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +L3GD20::measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_report; +#pragma pack(pop) + + gyro_report *report = &_reports[_next_report]; + + /* start the performance counter */ + perf_begin(_sample_perf); + + /* fetch data from the sensor */ + raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + report->timestamp = hrt_absolute_time(); + + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->z_raw = raw_report.z; + + report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + report->scaling = _gyro_range_scale; + report->range_rad_s = _gyro_range_rad_s; + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_report == _oldest_report) + INCREMENT(_oldest_report, _num_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + + /* stop the perf counter */ + perf_end(_sample_perf); +} + +void +L3GD20::print_info() +{ + perf_print_counter(_sample_perf); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace l3gd20 +{ + +L3GD20 *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_gyro = -1; + struct gyro_report g_report; + ssize_t sz; + + /* get the driver */ + fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd_gyro < 0) + err(1, "%s open failed", GYRO_DEVICE_PATH); + + /* reset to manual polling */ + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) + err(1, "immediate gyro read failed"); + + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +l3gd20_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + l3gd20::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + l3gd20::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + l3gd20::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + l3gd20::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk new file mode 100644 index 000000000..23e77e871 --- /dev/null +++ b/src/drivers/l3gd20/module.mk @@ -0,0 +1,6 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = l3gd20 +SRCS = l3gd20.cpp -- cgit v1.2.3 From b7e947cb3d53cbb0f9b194980a6a63588ba56bf2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 17 Apr 2013 11:22:08 -0700 Subject: Anti-Aliasing frequency of the LSM303D can now be read too, not just written --- src/drivers/lsm303d/lsm303d.cpp | 52 +++++++++++++++++++++++++++++++++++++---- 1 file changed, 48 insertions(+), 4 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8a64ee702..ba7316e55 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -321,7 +321,15 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned max_g); + int set_antialias_filter_bandwidth(unsigned bandwith); + + /** + * Get the LSM303D accel anti-alias filter. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * @return OK if the value was read and supported, ERROR otherwise. + */ + int get_antialias_filter_bandwidth(unsigned &bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -477,10 +485,10 @@ LSM303D::init() /* XXX should we enable FIFO? */ set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */ + set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ - mag_set_range(12); /* XXX: take highest sensor range of 12GA? */ + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); /* XXX test this when another mag is used */ @@ -687,6 +695,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX implement */ return -EINVAL; +// case ACCELIOCSLOWPASS: + case ACCELIOCGLOWPASS: + + unsigned bandwidth; + + if (OK == get_antialias_filter_bandwidth(bandwidth)) + return bandwidth; + else + return -EINVAL; + + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -942,6 +961,25 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) return OK; } +int +LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) +{ + uint8_t readbits = read_reg(ADDR_CTRL_REG2); + + if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) + bandwidth = 50; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) + bandwidth = 194; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) + bandwidth = 362; + else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) + bandwidth = 773; + else + return ERROR; + + return OK; +} + int LSM303D::set_samplerate(unsigned frequency) { @@ -1305,6 +1343,7 @@ test() int fd_accel = -1; struct accel_report a_report; ssize_t sz; + int filter_bandwidth; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1318,14 +1357,19 @@ test() if (sz != sizeof(a_report)) err(1, "immediate read failed"); + warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); warnx("accel x: \t%d\traw", (int)a_report.x_raw); warnx("accel y: \t%d\traw", (int)a_report.y_raw); warnx("accel z: \t%d\traw", (int)a_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + warnx("accel antialias filter bandwidth: fail"); + else + warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); int fd_mag = -1; struct mag_report m_report; -- cgit v1.2.3 From 5f2601836524055c3eb046535d53a38b0749ca52 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Apr 2013 22:25:50 -0700 Subject: Improve the implementation of CONFIG_FILE handling in firmware.mk --- makefiles/firmware.mk | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index a2227b5c4..f6057d2fc 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -66,6 +66,12 @@ # CONFIG: # Used to set the output filename; defaults to 'firmware'. # +# CONFIG_FILE: +# If set, overrides the configuration file search logic. Sets +# CONFIG to the name of the configuration file, strips any +# leading config_ prefix and any suffix. e.g. config_board_foo.mk +# results in CONFIG being set to 'board_foo'. +# # WORK_DIR: # Sets the directory in which the firmware will be built. Defaults # to the directory 'build' under the directory containing the @@ -115,13 +121,14 @@ include $(MK_DIR)/setup.mk # # Locate the configuration file # +ifneq ($(CONFIG_FILE),) +CONFIG := $(subst config_,,$(basename $(notdir $(CONFIG_FILE)))) +else +CONFIG_FILE := $(wildcard $(PX4_MK_DIR)/config_$(CONFIG).mk) +endif ifeq ($(CONFIG),) $(error Missing configuration name or file (specify with CONFIG=)) endif -CONFIG_FILE := $(firstword $(wildcard $(CONFIG)) $(wildcard $(PX4_MK_DIR)/config_$(CONFIG).mk)) -ifeq ($(CONFIG_FILE),) -$(error Can't find a config file called $(CONFIG) or $(PX4_MK_DIR)/config_$(CONFIG).mk) -endif export CONFIG include $(CONFIG_FILE) $(info % CONFIG = $(CONFIG)) -- cgit v1.2.3 From 94084ec22abd3c08cdd06783483e827ed8b7fd66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Apr 2013 22:27:55 +0200 Subject: Enable support for RAMTRON, enable support for EEPROM on FMU 1.x --- makefiles/config_px4fmu_default.mk | 3 +- makefiles/config_px4fmuv2_default.mk | 2 + nuttx/configs/px4fmu/nsh/appconfig | 1 - nuttx/configs/px4fmuv2/nsh/appconfig | 6 +- nuttx/configs/px4fmuv2/nsh/defconfig | 4 + nuttx/drivers/mtd/ramtron.c | 8 + src/drivers/boards/px4fmuv2/px4fmu_init.c | 17 +- src/systemcmds/eeprom/24xxxx_mtd.c | 571 ++++++++++++++++++++++++++++++ src/systemcmds/eeprom/eeprom.c | 265 ++++++++++++++ src/systemcmds/eeprom/module.mk | 6 + src/systemcmds/ramtron/module.mk | 6 + src/systemcmds/ramtron/ramtron.c | 268 ++++++++++++++ 12 files changed, 1142 insertions(+), 15 deletions(-) create mode 100644 src/systemcmds/eeprom/24xxxx_mtd.c create mode 100644 src/systemcmds/eeprom/eeprom.c create mode 100644 src/systemcmds/eeprom/module.mk create mode 100644 src/systemcmds/ramtron/module.mk create mode 100644 src/systemcmds/ramtron/ramtron.c diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1a6c91b26..291711820 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -11,6 +11,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # Board support modules # MODULES += drivers/px4fmu +MODULES += systemcmds/eeprom # # Transitional support - add commands from the NuttX export archive. @@ -36,7 +37,6 @@ BUILTIN_COMMANDS := \ $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_main ) \ $(call _B, control_demo, , 2048, control_demo_main ) \ $(call _B, delay_test, , 2048, delay_test_main ) \ - $(call _B, eeprom, , 4096, eeprom_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 ) \ @@ -66,4 +66,5 @@ BUILTIN_COMMANDS := \ $(call _B, tests, , 12000, tests_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/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d296c5379..6d583bd5f 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -14,6 +14,7 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/lsm303d MODULES += drivers/px4fmu MODULES += drivers/rgbled +MODULES += systemcmds/ramtron # # Transitional support - add commands from the NuttX export archive. @@ -61,4 +62,5 @@ BUILTIN_COMMANDS := \ $(call _B, tests, , 12000, tests_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/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index d642b4692..9092e9541 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -65,7 +65,6 @@ CONFIGURED_APPS += systemcmds/perf CONFIGURED_APPS += systemcmds/top CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer -CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index fff140673..9e3f50a13 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -65,9 +65,7 @@ CONFIGURED_APPS += systemcmds/perf CONFIGURED_APPS += systemcmds/top CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer -# No I2C EEPROM - need new param interface -#CONFIGURED_APPS += systemcmds/eeprom -#CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/param CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/preflight_check @@ -95,10 +93,8 @@ CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors ifneq ($(CONFIG_APM),y) -#CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control -#CONFIGURED_APPS += fixedwing_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index beeb47551..d10309580 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -365,6 +365,10 @@ CONFIG_I2C_RESET=y # XXX fixed per-transaction timeout CONFIG_STM32_I2CTIMEOMS=10 +# +# MTD support +# +CONFIG_MTD=y # XXX re-enable after integration testing diff --git a/nuttx/drivers/mtd/ramtron.c b/nuttx/drivers/mtd/ramtron.c index 34273bccf..45aff59cc 100644 --- a/nuttx/drivers/mtd/ramtron.c +++ b/nuttx/drivers/mtd/ramtron.c @@ -161,6 +161,14 @@ struct ramtron_dev_s static struct ramtron_parts_s ramtron_parts[] = { + { + "FM25V01", /* name */ + 0x21, /* id1 */ + 0x00, /* id2 */ + 16L*1024L, /* size */ + 2, /* addr_len */ + 40000000 /* speed */ + }, { "FM25V02", /* name */ 0x22, /* id1 */ diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 1d99f15bf..2fd3a2c1b 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -192,12 +192,12 @@ __EXPORT int nsh_archinitialize(void) spi1 = up_spiinitialize(1); if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\r\n"); + message("[boot] FAILED to initialize SPI port 1\n"); up_ledon(LED_AMBER); return -ENODEV; } - // Default SPI1 to 1MHz and de-assert the known chip selects. + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); @@ -206,11 +206,10 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\r\n"); + message("[boot] Successfully initialized SPI port 1\n"); /* Get the SPI port for the FRAM */ - message("[boot] Initializing SPI port 2\n"); spi2 = up_spiinitialize(2); if (!spi2) { @@ -219,11 +218,13 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - message("[boot] Successfully initialized SPI port 2\n"); - - /* XXX need a driver to bind the FRAM to */ + /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 375000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); - //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); + message("[boot] Successfully initialized SPI port 2\n"); return OK; } diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c new file mode 100644 index 000000000..e34be44e3 --- /dev/null +++ b/src/systemcmds/eeprom/24xxxx_mtd.c @@ -0,0 +1,571 @@ +/************************************************************************************ + * Driver for 24xxxx-style I2C EEPROMs. + * + * Adapted from: + * + * drivers/mtd/at24xx.c + * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) + * + * Copyright (C) 2011 Li Zhuoyi. All rights reserved. + * Author: Li Zhuoyi + * History: 0.1 2011-08-20 initial version + * + * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn + * + * Derived from drivers/mtd/m25px.c + * + * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "systemlib/perf_counter.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* + * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be + * omitted in order to avoid building the AT24XX driver as well. + */ + +/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ + +#ifndef CONFIG_AT24XX_SIZE +# warning "Assuming AT24 size 64" +# define CONFIG_AT24XX_SIZE 64 +#endif +#ifndef CONFIG_AT24XX_ADDR +# warning "Assuming AT24 address of 0x50" +# define CONFIG_AT24XX_ADDR 0x50 +#endif + +/* Get the part configuration based on the size configuration */ + +#if CONFIG_AT24XX_SIZE == 32 +# define AT24XX_NPAGES 128 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 48 +# define AT24XX_NPAGES 192 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 64 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 128 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 64 +#elif CONFIG_AT24XX_SIZE == 256 +# define AT24XX_NPAGES 512 +# define AT24XX_PAGESIZE 64 +#endif + +/* For applications where a file system is used on the AT24, the tiny page sizes + * will result in very inefficient FLASH usage. In such cases, it is better if + * blocks are comprised of "clusters" of pages so that the file system block + * size is, say, 256 or 512 bytes. In any event, the block size *must* be an + * even multiple of the pages. + */ + +#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE +# warning "Assuming driver block size is the same as the FLASH page size" +# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE +#endif + +/* The AT24 does not respond on the bus during write cycles, so we depend on a long + * timeout to detect problems. The max program time is typically ~5ms. + */ +#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS +# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/* This type represents the state of the MTD device. The struct mtd_dev_s + * must appear at the beginning of the definition so that you can freely + * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. + */ + +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +/* MTD driver methods */ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buf); +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR const uint8_t *buf); +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); + +void at24c_test(void); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/* At present, only a single AT24 part is supported. In this case, a statically + * allocated state structure may be used. + */ + +static struct at24c_dev_s g_at24c; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +static int at24c_eraseall(FAR struct at24c_dev_s *priv) +{ + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; +} + +/************************************************************************************ + * Name: at24c_erase + ************************************************************************************/ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) +{ + /* EEprom need not erase */ + + return (int)nblocks; +} + +/************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + +/************************************************************************************ + * Name: at24c_bread + ************************************************************************************/ + +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_bwrite + * + * Operates on MTD block's and translates to FLASH pages + * + ************************************************************************************/ + +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, + FAR const uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); + + for (;;) { + + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); + + if (ret >= 0) + break; + + fvdbg("write stall"); + usleep(1000); + + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_ioctl + ************************************************************************************/ + +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; +#else + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; +#endif + ret = OK; + + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; + + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } + + return ret; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: at24c_initialize + * + * Description: + * Create an initialize MTD device instance. MTD devices are not registered + * in the file system, but are created as instances that can be bound to + * other functions (such as a block or character driver front end). + * + ************************************************************************************/ + +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; +} + +/* + * XXX: debug hackery + */ +int at24c_nuke(void) +{ + return at24c_eraseall(&g_at24c); +} diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c new file mode 100644 index 000000000..49da51358 --- /dev/null +++ b/src/systemcmds/eeprom/eeprom.c @@ -0,0 +1,265 @@ +/**************************************************************************** + * + * 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 eeprom.c + * + * EEPROM service and utility app. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM +#endif + +__EXPORT int eeprom_main(int argc, char *argv[]); + +static void eeprom_attach(void); +static void eeprom_start(void); +static void eeprom_erase(void); +static void eeprom_ioctl(unsigned operation); +static void eeprom_save(const char *name); +static void eeprom_load(const char *name); +static void eeprom_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *eeprom_mtd; + +int eeprom_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + eeprom_start(); + + if (!strcmp(argv[1], "save_param")) + eeprom_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + eeprom_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + eeprom_erase(); + + if (!strcmp(argv[1], "test")) + eeprom_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + eeprom_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + eeprom_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); +} + + +static void +eeprom_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + eeprom_mtd = at24c_initialize(i2c); + if (eeprom_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (eeprom_mtd == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} + +static void +eeprom_start(void) +{ + int ret; + + if (started) + errx(1, "EEPROM already mounted"); + + if (!attached) + eeprom_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(eeprom_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); + + /* mount the EEPROM */ + ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); + + started = true; + warnx("mounted EEPROM at /eeprom"); + exit(0); +} + +extern int at24c_nuke(void); + +static void +eeprom_erase(void) +{ + if (!attached) + eeprom_attach(); + + if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +eeprom_ioctl(unsigned operation) +{ + int fd; + + fd = open("/eeprom/.", 0); + + if (fd < 0) + err(1, "open /eeprom"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +eeprom_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/eeprom/parameters'"); + + warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +eeprom_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/eeprom/parameters'"); + + warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +extern void at24c_test(void); + +static void +eeprom_test(void) +{ + at24c_test(); + exit(0); +} diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk new file mode 100644 index 000000000..3b4fc0479 --- /dev/null +++ b/src/systemcmds/eeprom/module.mk @@ -0,0 +1,6 @@ +# +# EEPROM file system driver +# + +MODULE_COMMAND = eeprom +SRCS = 24xxxx_mtd.c eeprom.c diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk new file mode 100644 index 000000000..e4eb1d143 --- /dev/null +++ b/src/systemcmds/ramtron/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = ramtron +SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c new file mode 100644 index 000000000..5e9499c55 --- /dev/null +++ b/src/systemcmds/ramtron/ramtron.c @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (C) 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 ramtron.c + * + * ramtron service and utility app. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int ramtron_main(int argc, char *argv[]); + +static void ramtron_attach(void); +static void ramtron_start(void); +static void ramtron_erase(void); +static void ramtron_ioctl(unsigned operation); +static void ramtron_save(const char *name); +static void ramtron_load(const char *name); +static void ramtron_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *ramtron_mtd; + +int ramtron_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + ramtron_start(); + + if (!strcmp(argv[1], "save_param")) + ramtron_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + ramtron_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + ramtron_erase(); + + if (!strcmp(argv[1], "test")) + ramtron_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + ramtron_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + ramtron_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); + + +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + // xxx set in ramtron driver, leave this out +// SPI_SETFREQUENCY(spi, 4000000); + SPI_SETFREQUENCY(spi, 375000000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + ramtron_mtd = ramtron_initialize(spi); + if (ramtron_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: ramtron needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (ramtron_mtd == NULL) + errx(1, "failed to initialize ramtron driver"); + + attached = true; +} + +static void +ramtron_start(void) +{ + int ret; + + if (started) + errx(1, "ramtron already mounted"); + + if (!attached) + ramtron_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(ramtron_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); + + /* mount the ramtron */ + ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /ramtron - erase ramtron to reformat"); + + started = true; + warnx("mounted ramtron at /ramtron"); + exit(0); +} + +//extern int at24c_nuke(void); + +static void +ramtron_erase(void) +{ + if (!attached) + ramtron_attach(); + +// if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +ramtron_ioctl(unsigned operation) +{ + int fd; + + fd = open("/ramtron/.", 0); + + if (fd < 0) + err(1, "open /ramtron"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +ramtron_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +ramtron_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +//extern void at24c_test(void); + +static void +ramtron_test(void) +{ +// at24c_test(); + exit(0); +} -- cgit v1.2.3 From b149b834c835190fbb3f7e1914346d5e0620036d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Apr 2013 22:56:25 +0200 Subject: Initial attempt at getting SDIO to work --- nuttx/configs/px4fmuv2/include/board.h | 11 ++++++++++ nuttx/configs/px4fmuv2/nsh/defconfig | 35 +++++++++++++++++++++++++++---- src/drivers/boards/px4fmuv2/px4fmu_init.c | 25 ++++++++++++++++++++++ 3 files changed, 67 insertions(+), 4 deletions(-) diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index fd8f78b80..be4cdcdfd 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -181,6 +181,17 @@ # define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + /* High-resolution timer */ #ifdef CONFIG_HRT_TIMER diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index beeb47551..d2f711b2d 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -192,7 +192,7 @@ CONFIG_STM32_USART6=y CONFIG_STM32_ADC1=y CONFIG_STM32_ADC2=n CONFIG_STM32_ADC3=n -CONFIG_STM32_SDIO=n +CONFIG_STM32_SDIO=y CONFIG_STM32_SPI1=y CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM9=y @@ -780,13 +780,40 @@ CONFIG_FS_BINFS=y # CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. # Default is 20MHz, current setting 24 MHz # -CONFIG_MMCSD=n +#CONFIG_MMCSD=n # XXX need to rejig this for SDIO #CONFIG_MMCSD_SPI=y #CONFIG_MMCSD_NSLOTS=1 #CONFIG_MMCSD_READONLY=n #CONFIG_MMCSD_SPICLOCK=24000000 +# +# Maintain legacy build behavior (revisit) +# + +CONFIG_MMCSD=y +CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y + +# +# SPI-based MMC/SD driver +# +CONFIG_MMCSD_NSLOTS=1 +CONFIG_MMCSD_READONLY=n +CONFIG_MMCSD_SPICLOCK=12500000 + +# +# STM32 SDIO-based MMC/SD driver +# +CONFIG_SDIO_DMA=y +#CONFIG_SDIO_PRI=128 +#CONFIG_SDIO_DMAPRIO +#CONFIG_SDIO_WIDTH_D1_ONLY +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_MMCSUPPORT=n +CONFIG_MMCSD_HAVECARDDETECT=n + # # Block driver buffering # @@ -1004,8 +1031,8 @@ CONFIG_NSH_FATMOUNTPT=/tmp # Architecture-specific NSH options # #CONFIG_NSH_MMCSDSPIPORTNO=3 -#CONFIG_NSH_MMCSDSLOTNO=0 -#CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 # diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 1d99f15bf..2222e59a8 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -128,6 +129,7 @@ __EXPORT void stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; #include @@ -225,5 +227,28 @@ __EXPORT int nsh_archinitialize(void) //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); + #ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { + message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { + message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + + message("[boot] Initialized SDIO\n"); + #endif + return OK; } -- cgit v1.2.3 From 5abae2c78d0f7f41e1340fe5e396936da2c7a580 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 09:14:26 +0400 Subject: Publish manual_sas_mode immediately, SAS modes switch order changed to more safe --- apps/commander/commander.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7c0a25f86..3c74c15ef 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1510,6 +1510,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -1931,8 +1932,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + /* bottom stick position, set default */ + /* this MUST be mapped to extremal position to switch easy in case of emergency */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { @@ -1940,8 +1942,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + /* center stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + } + + if (current_status.manual_sas_mode != manual_sas_mode) { + /* publish SAS mode changes immediately */ + manual_sas_mode = current_status.manual_sas_mode; + state_changed = true; } /* -- cgit v1.2.3 From 57cca9dbb41a1803c3cdb61151947e99625c59cb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 12:33:02 +0400 Subject: Abs yaw and other bugs fixed --- .../multirotor_att_control_main.c | 199 +++++++++++---------- .../multirotor_pos_control.c | 152 +++++++++------- 2 files changed, 194 insertions(+), 157 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 09710f0fc..258299828 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -241,123 +241,140 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { + /* Control attitude, update attitude setpoint depending on SAS mode: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS: roll, pitch, yaw, thrust + * VEHICLE_MANUAL_SAS_MODE_ALTITUDE: roll, pitch, yaw + * VEHICLE_MANUAL_SAS_MODE_SIMPLE: yaw + * */ + + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { - /* direct manual input */ - if (state.flag_control_attitude_enabled) { - - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; + static bool rc_loss_first_time = true; - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* Don't reset attitude setpoint in SIMPLE SAS mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* Don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.thrust = failsafe_throttle; - } else { - att_sp.thrust = 0.0f; + } else { + att_sp.thrust = 0.0f; + } } + } - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; - rc_loss_first_time = false; + rc_loss_first_time = false; - } else { - rc_loss_first_time = true; + } else { + rc_loss_first_time = true; - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + /* control yaw in all SAS modes */ + /* set yaw if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; - } + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + } else { + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && ( + state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS || + manual.throttle > 0.3f)) { rates_sp.yaw = manual.yaw; control_yaw_position = false; + first_time_after_yaw_speed_control = true; } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } + + control_yaw_position = true; } } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); } - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* don't update attitude setpoint in SIMPLE SAS mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* don't set throttle in alt hold modes */ + att_sp.thrust = manual.throttle; + } } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + att_sp.timestamp = hrt_absolute_time(); + } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; - - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } + + /* STEP 2: publish the controller output */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); } } } diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index d2b6685ac..7757a78c1 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -162,6 +162,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -186,18 +187,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int paramcheck_counter = 0; while (!thread_should_exit) { - /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &status); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (status.state_machine == SYSTEM_STATE_AUTO) { - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - } /* check parameters at 1 Hz*/ paramcheck_counter++; @@ -210,26 +200,58 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } - if (status.state_machine == SYSTEM_STATE_MANUAL) { - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - hrt_abstime t = hrt_absolute_time(); - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); - } + /* Check if controller should act */ + bool act = status.flag_system_armed && ( + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); + + if (act) { + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + } - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - /* move altitude setpoint by manual controls */ + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } + + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + reset_sp_pos = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + char str[80]; + sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, str); + } + + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + + if (status.flag_control_manual_enabled) { + /* move altitude setpoint with manual controls */ float alt_ctl = manual.throttle - 0.5f; if (fabs(alt_ctl) < alt_ctl_dz) { alt_ctl = 0.0f; @@ -245,48 +267,46 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { local_pos_sp.z = local_pos.z - err_linear_limit; } - /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } - /* PID for altitude rate */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; - } else { - /* integrate only in linear area (with 20% gap) and not on min/max throttle */ - if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO add position controller - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; - } else { - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; + // TODO move position setpoint with manual controls } - att_sp.thrust = thrust_ctl; - att_sp.timestamp = hrt_absolute_time(); - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; + } + + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* P and D components */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + // TODO add position controller } else { - reset_sp_alt = true; + reset_sp_pos = true; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { + /* publish local position setpoint in manual mode */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; } else { reset_sp_alt = true; + reset_sp_pos = true; } /* run at approximately 50 Hz */ -- cgit v1.2.3 From 10dfd2ba393089e5008978eb0469e8c1289d5917 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 22:38:38 +0400 Subject: dt calculation bug fixed --- apps/multirotor_pos_control/multirotor_pos_control.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7757a78c1..ac7645764 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -214,6 +214,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { status.state_machine == SYSTEM_STATE_AUTO ); + hrt_abstime t = hrt_absolute_time(); + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + t_prev = t; if (act) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -223,7 +231,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); } - hrt_abstime t = hrt_absolute_time(); if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; @@ -242,12 +249,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, str); } - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; if (status.flag_control_manual_enabled) { @@ -303,7 +304,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; } else { reset_sp_alt = true; reset_sp_pos = true; -- cgit v1.2.3 From 7e8d8f9e7226bcc04a5f8dd4b01c9a6a4f1f9910 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 12:49:56 -0700 Subject: Call sub-makes with -r to make them start faster (mostly on Windows, where this inhibits an enormous amount of silly scanning for things). Force non-parallel builds for the NuttX archives. --- Makefile | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 0b8a65d94..2347b09bf 100644 --- a/Makefile +++ b/Makefile @@ -96,7 +96,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: @echo %%%% Building $(config) in $(work_dir) @echo %%%% $(Q) mkdir -p $(work_dir) - $(Q) make -C $(work_dir) \ + $(Q) make -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -119,15 +119,21 @@ NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: $(NUTTX_ARCHIVES) +# We cannot build these parallel; note that we also force -j1 for the +# sub-make invocations. +ifneq ($(filter archives,$(MAKECMDGOALS)),) +.NOTPARALLEL: +endif + $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh) $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) @echo %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @echo %% Exporting NuttX for $(board) - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) export + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ -- cgit v1.2.3 From 754a11f4fcad7c9d6c2d771cf2f5ff58f90e2224 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 12:51:07 -0700 Subject: Use a .elf suffix for the ELF object file (seems more sensible that way) Detect the case where PX4_BASE contains spaces and stop before we cause any more damage. Call sub-makes with -r to improve startup time. --- makefiles/firmware.mk | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f6057d2fc..83bba3a9b 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -64,7 +64,10 @@ # path to this file. # # CONFIG: -# Used to set the output filename; defaults to 'firmware'. +# Used when searching for the configuration file, and available +# to module Makefiles to select optional features. +# If not set, CONFIG_FILE must be set and CONFIG will be derived +# automatically from it. # # CONFIG_FILE: # If set, overrides the configuration file search logic. Sets @@ -98,11 +101,14 @@ # If PX4_BASE wasn't set previously, work out what it should be # and set it here now. # -MK_DIR ?= $(dir $(lastword $(MAKEFILE_LIST))) +MK_DIR ?= $(dir $(lastword $(MAKEFILE_LIST))) ifeq ($(PX4_BASE),) export PX4_BASE := $(abspath $(MK_DIR)/..) endif $(info % PX4_BASE = $(PX4_BASE)) +ifneq ($(words $(PX4_BASE)),1) +$(error Cannot build when the PX4_BASE path contains one or more space characters.) +endif # # Set a default target so that included makefiles or errors here don't @@ -218,10 +224,7 @@ MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module $(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@) $(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath)) $(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) - @$(ECHO) %% - @$(ECHO) %% Building module using $(mkfile) - @$(ECHO) %% - $(Q) $(MAKE) -f $(PX4_MK_DIR)module.mk \ + $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ MODULE_WORK_DIR=$(dir $@) \ MODULE_OBJ=$@ \ MODULE_MK=$(mkfile) \ @@ -237,7 +240,7 @@ $(MODULE_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@) $(MODULE_CLEANS): mkfile = $(patsubst %clean,%module.mk,$(relpath)) $(MODULE_CLEANS): @$(ECHO) %% cleaning using $(mkfile) - $(Q) $(MAKE) -f $(PX4_MK_DIR)module.mk \ + $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ MODULE_WORK_DIR=$(dir $@) \ MODULE_MK=$(mkfile) \ clean @@ -372,7 +375,7 @@ endif # PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin -PRODUCT_SYM = $(WORK_DIR)firmware.sym +PRODUCT_ELF = $(WORK_DIR)firmware.elf .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -407,10 +410,10 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN) --git_identity $(PX4_BASE) \ --image $< > $@ -$(PRODUCT_BIN): $(PRODUCT_SYM) +$(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) -$(PRODUCT_SYM): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) +$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) $(call LINK,$@,$(OBJS) $(MODULE_OBJS)) # @@ -428,7 +431,7 @@ upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) .PHONY: clean clean: $(MODULE_CLEANS) @$(ECHO) %% cleaning - $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_SYM) + $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_ELF) $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) -- cgit v1.2.3 From a7cf9e2a366b3ac4d92e8e12da23308e285d6ead Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 14:57:15 -0700 Subject: Make 'make upload' work --- Makefile | 17 ++++++++++++++++- Tools/px_uploader.py | 2 +- makefiles/firmware.mk | 4 +++- makefiles/module.mk | 3 ++- makefiles/upload.mk | 9 ++++++--- 5 files changed, 28 insertions(+), 7 deletions(-) diff --git a/Makefile b/Makefile index 2347b09bf..c3ef7ad0d 100644 --- a/Makefile +++ b/Makefile @@ -63,11 +63,26 @@ MQUIET = --no-print-directory # # If the user has listed a config as a target, strip it out and override CONFIGS. # +FIRMWARE_GOAL = firmware EXPLICIT_CONFIGS := $(filter $(CONFIGS),$(MAKECMDGOALS)) ifneq ($(EXPLICIT_CONFIGS),) CONFIGS := $(EXPLICIT_CONFIGS) .PHONY: $(EXPLICIT_CONFIGS) $(EXPLICIT_CONFIGS): all + +# +# If the user has asked to upload, they must have also specified exactly one +# config. +# +ifneq ($(filter upload,$(MAKECMDGOALS)),) +ifneq ($(words $(EXPLICIT_CONFIGS)),1) +$(error In order to upload, exactly one board config must be specified) +endif +FIRMWARE_GOAL = upload +.PHONY: upload +upload: + @: +endif endif # @@ -100,7 +115,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ - firmware + $(FIRMWARE_GOAL) # # Build the NuttX export archives. diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce388d71..423bdb42a 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -167,7 +167,7 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.25) + self.port = serial.Serial(portname, baudrate, timeout=0.5) def close(self): if self.port is not None: diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 83bba3a9b..fff0e1c65 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -424,7 +424,8 @@ $(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKF upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(Q) $(MAKE) -f $(PX4_MK_DIR)/upload.mk \ METHOD=serial \ - PRODUCT=$(PRODUCT) \ + CONFIG=$(CONFIG) \ + BOARD=$(BOARD) \ BUNDLE=$(PRODUCT_BUNDLE) \ BIN=$(PRODUCT_BIN) @@ -435,6 +436,7 @@ clean: $(MODULE_CLEANS) $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) + # # DEP_INCLUDES is defined by the toolchain include in terms of $(OBJS) # diff --git a/makefiles/module.mk b/makefiles/module.mk index 1db0f6fee..e2a1041e0 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -102,7 +102,7 @@ ifeq ($(MODULE_MK),) $(error No module makefile specified) endif -$(info % MODULE_MK = $(MODULE_MK)) +$(info %% MODULE_MK = $(MODULE_MK)) # # Get the board/toolchain config @@ -147,6 +147,7 @@ $(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).* $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) @$(REMOVE) -f $(exclude) @$(MKDIR) -p $(dir $@) + @echo "CMD: $(command)" $(Q) $(TOUCH) $@ endif diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 5308aaa3e..a1159d157 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -21,11 +21,14 @@ ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" endif -.PHONY: all upload-$(METHOD)-$(PRODUCT) -all: upload-$(METHOD)-$(PRODUCT) +.PHONY: all upload-$(METHOD)-$(BOARD) +all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu: $(BUNDLE) $(UPLOADER) - @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(PRODUCT_BUNDLE) + @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + +upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) + @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # # JTAG firmware uploading with OpenOCD -- cgit v1.2.3 From 7e0f8b3edaf584a48cd3bc3351e3205fd0106cdc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Apr 2013 15:18:10 -0700 Subject: Formatting changes to make the Python style checker happy (copied from the bootloader project). Increase the erase timeout to avoid issues with large/slow flash. --- Tools/px_uploader.py | 679 ++++++++++++++++++++++++++------------------------- 1 file changed, 340 insertions(+), 339 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 423bdb42a..d2ebf1698 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -41,20 +41,19 @@ # The uploader uses the following fields from the firmware file: # # image -# The firmware that will be uploaded. +# The firmware that will be uploaded. # image_size -# The size of the firmware in bytes. +# The size of the firmware in bytes. # board_id -# The board for which the firmware is intended. +# The board for which the firmware is intended. # board_revision -# Currently only used for informational purposes. +# Currently only used for informational purposes. # import sys import argparse import binascii import serial -import os import struct import json import zlib @@ -64,292 +63,294 @@ import array from sys import platform as _platform + class firmware(object): - '''Loads a firmware file''' - - desc = {} - image = bytes() - crctab = array.array('I', [ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ]) - crcpad = bytearray('\xff\xff\xff\xff') - - def __init__(self, path): - - # read the file - f = open(path, "r") - self.desc = json.load(f) - f.close() - - self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) - - # pad image to 4-byte length - while ((len(self.image) % 4) != 0): - self.image.append('\xff') - - def property(self, propname): - return self.desc[propname] - - def __crc32(self, bytes, state): - for byte in bytes: - index = (state ^ byte) & 0xff - state = self.crctab[index] ^ (state >> 8) - return state - - def crc(self, padlen): - state = self.__crc32(self.image, int(0)) - for i in range(len(self.image), (padlen - 1), 4): - state = self.__crc32(self.crcpad, state) - return state + '''Loads a firmware file''' + + desc = {} + image = bytes() + crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) + crcpad = bytearray('\xff\xff\xff\xff') + + def __init__(self, path): + + # read the file + f = open(path, "r") + self.desc = json.load(f) + f.close() + + self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) + + # pad image to 4-byte length + while ((len(self.image) % 4) != 0): + self.image.append('\xff') + + def property(self, propname): + return self.desc[propname] + + def __crc32(self, bytes, state): + for byte in bytes: + index = (state ^ byte) & 0xff + state = self.crctab[index] ^ (state >> 8) + return state + + def crc(self, padlen): + state = self.__crc32(self.image, int(0)) + for i in range(len(self.image), (padlen - 1), 4): + state = self.__crc32(self.crcpad, state) + return state + class uploader(object): - '''Uploads a firmware file to the PX FMU bootloader''' - - # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) - - # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ - - # command bytes - NOP = chr(0x00) # guaranteed to be discarded by the bootloader - GET_SYNC = chr(0x21) - GET_DEVICE = chr(0x22) - CHIP_ERASE = chr(0x23) - CHIP_VERIFY = chr(0x24) # rev2 only - PROG_MULTI = chr(0x27) - READ_MULTI = chr(0x28) # rev2 only - GET_CRC = chr(0x29) # rev3+ - REBOOT = chr(0x30) - - INFO_BL_REV = chr(1) # bootloader protocol revision - BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol - INFO_BOARD_ID = chr(2) # board type - INFO_BOARD_REV = chr(3) # board revision - INFO_FLASH_SIZE = chr(4) # max firmware size in bytes - - PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 - READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 - - def __init__(self, portname, baudrate): - # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.5) - - def close(self): - if self.port is not None: - self.port.close() - - def __send(self, c): -# print("send " + binascii.hexlify(c)) - self.port.write(str(c)) - - def __recv(self, count = 1): - c = self.port.read(count) - if len(c) < 1: - raise RuntimeError("timeout waiting for data") -# print("recv " + binascii.hexlify(c)) - return c - - def __recv_int(self): - raw = self.__recv(4) - val = struct.unpack("= 3: - self.__getSync() - - # split a sequence into a list of size-constrained pieces - def __split_len(self, seq, length): - return [seq[i:i+length] for i in range(0, len(seq), length)] - - # upload code - def __program(self, fw): - code = fw.image - groups = self.__split_len(code, uploader.PROG_MULTI_MAX) - for bytes in groups: - self.__program_multi(bytes) - - # verify code - def __verify_v2(self, fw): - self.__send(uploader.CHIP_VERIFY - + uploader.EOC) - self.__getSync() - code = fw.image - groups = self.__split_len(code, uploader.READ_MULTI_MAX) - for bytes in groups: - if (not self.__verify_multi(bytes)): - raise RuntimeError("Verification failed") - - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) - self.__send(uploader.GET_CRC - + uploader.EOC) - report_crc = self.__recv_int() - self.__getSync() - if report_crc != expect_crc: - print(("Expected 0x%x" % expect_crc)) - print(("Got 0x%x" % report_crc)) - raise RuntimeError("Program CRC failed") - - # get basic data about the board - def identify(self): - # make sure we are in sync before starting - self.__sync() - - # get the bootloader protocol ID first - self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) - if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): - print(("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)) - raise RuntimeError("Bootloader protocol mismatch") - - self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) - self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) - self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) - - # upload the firmware - def upload(self, fw): - # Make sure we are doing the right thing - if self.board_type != fw.property('board_id'): - raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).") - if self.fw_maxsize < fw.property('image_size'): - raise RuntimeError("Firmware image is too large for this board") - - print("erase...") - self.__erase() - - print("program...") - self.__program(fw) - - print("verify...") - if self.bl_rev == 2: - self.__verify_v2(fw) - else: - self.__verify_v3(fw) - - print("done, rebooting.") - self.__reboot() - self.port.close() - + '''Uploads a firmware file to the PX FMU bootloader''' + + # protocol bytes + INSYNC = chr(0x12) + EOC = chr(0x20) + + # reply bytes + OK = chr(0x10) + FAILED = chr(0x11) + INVALID = chr(0x13) # rev3+ + + # command bytes + NOP = chr(0x00) # guaranteed to be discarded by the bootloader + GET_SYNC = chr(0x21) + GET_DEVICE = chr(0x22) + CHIP_ERASE = chr(0x23) + CHIP_VERIFY = chr(0x24) # rev2 only + PROG_MULTI = chr(0x27) + READ_MULTI = chr(0x28) # rev2 only + GET_CRC = chr(0x29) # rev3+ + REBOOT = chr(0x30) + + INFO_BL_REV = chr(1) # bootloader protocol revision + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 3 # maximum supported bootloader protocol + INFO_BOARD_ID = chr(2) # board type + INFO_BOARD_REV = chr(3) # board revision + INFO_FLASH_SIZE = chr(4) # max firmware size in bytes + + PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + + def __init__(self, portname, baudrate): + # open the port, keep the default timeout short so we can poll quickly + self.port = serial.Serial(portname, baudrate, timeout=0.5) + + def close(self): + if self.port is not None: + self.port.close() + + def __send(self, c): +# print("send " + binascii.hexlify(c)) + self.port.write(str(c)) + + def __recv(self, count=1): + c = self.port.read(count) + if len(c) < 1: + raise RuntimeError("timeout waiting for data") +# print("recv " + binascii.hexlify(c)) + return c + + def __recv_int(self): + raw = self.__recv(4) + val = struct.unpack("= 3: + self.__getSync() + + # split a sequence into a list of size-constrained pieces + def __split_len(self, seq, length): + return [seq[i:i+length] for i in range(0, len(seq), length)] + + # upload code + def __program(self, fw): + code = fw.image + groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + for bytes in groups: + self.__program_multi(bytes) + + # verify code + def __verify_v2(self, fw): + self.__send(uploader.CHIP_VERIFY + + uploader.EOC) + self.__getSync() + code = fw.image + groups = self.__split_len(code, uploader.READ_MULTI_MAX) + for bytes in groups: + if (not self.__verify_multi(bytes)): + raise RuntimeError("Verification failed") + + def __verify_v3(self, fw): + expect_crc = fw.crc(self.fw_maxsize) + self.__send(uploader.GET_CRC + + uploader.EOC) + report_crc = self.__recv_int() + self.__getSync() + if report_crc != expect_crc: + print("Expected 0x%x" % expect_crc) + print("Got 0x%x" % report_crc) + raise RuntimeError("Program CRC failed") + + # get basic data about the board + def identify(self): + # make sure we are in sync before starting + self.__sync() + + # get the bootloader protocol ID first + self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) + if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): + print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + raise RuntimeError("Bootloader protocol mismatch") + + self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) + self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) + self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + + # upload the firmware + def upload(self, fw): + # Make sure we are doing the right thing + if self.board_type != fw.property('board_id'): + raise RuntimeError("Firmware not suitable for this board") + if self.fw_maxsize < fw.property('image_size'): + raise RuntimeError("Firmware image is too large for this board") + + print("erase...") + self.__erase() + + print("program...") + self.__program(fw) + + print("verify...") + if self.bl_rev == 2: + self.__verify_v2(fw) + else: + self.__verify_v3(fw) + + print("done, rebooting.") + self.__reboot() + self.port.close() + # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") @@ -360,57 +361,57 @@ args = parser.parse_args() # Load the firmware file fw = firmware(args.firmware) -print(("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))) +print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) # Spin waiting for a device to show up while True: - for port in args.port.split(","): - - #print("Trying %s" % port) - - # create an uploader attached to the port - try: - if "linux" in _platform: - # Linux, don't open Mac OS and Win ports - if not "COM" in port and not "tty.usb" in port: - up = uploader(port, args.baud) - elif "darwin" in _platform: - # OS X, don't open Windows and Linux ports - if not "COM" in port and not "ACM" in port: - up = uploader(port, args.baud) - elif "win" in _platform: - # Windows, don't open POSIX ports - if not "/" in port: - up = uploader(port, args.baud) - except: - # open failed, rate-limit our attempts - time.sleep(0.05) - - # and loop to the next port - continue - - # port is open, try talking to it - try: - # identify the bootloader - up.identify() - print(("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))) - - except: - # most probably a timeout talking to the port, no bootloader - continue - - try: - # ok, we have a bootloader, try flashing it - up.upload(fw) - - except RuntimeError as ex: - - # print the error - print(("ERROR: %s" % ex.args)) - - finally: - # always close the port - up.close() - - # we could loop here if we wanted to wait for more boards... - sys.exit(0) + for port in args.port.split(","): + + #print("Trying %s" % port) + + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + + except: + # most probably a timeout talking to the port, no bootloader + continue + + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) + + except RuntimeError as ex: + + # print the error + print("ERROR: %s" % ex.args) + + finally: + # always close the port + up.close() + + # we could loop here if we wanted to wait for more boards... + sys.exit(0) -- cgit v1.2.3 From 8fcbb4f669d8c9003f778f35a94278383e0360ac Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 22 Apr 2013 22:56:18 -0700 Subject: Merge SDIO changes and hack config to make it work. We need to resolve the DMA-safe memory allocation story, but until then let's disable the CCM. We still have as much RAM as the v1.x boards in this mode. --- nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c | 2 +- nuttx/configs/px4fmuv2/nsh/defconfig | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c index 40fce8cb5..5140ee4f9 100644 --- a/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c +++ b/nuttx/arch/arm/src/stm32/stm32f40xxx_dma.c @@ -130,7 +130,7 @@ static struct stm32_dma_s g_dma[DMA_NSTREAMS] = .stream = 3, .irq = STM32_IRQ_DMA1S3, .shift = DMA_INT_STREAM3_SHIFT, - .base = STM32_DMA1_BASE + STM32_DMA_OFFSET(4), + .base = STM32_DMA1_BASE + STM32_DMA_OFFSET(3), }, { .stream = 4, diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 835998072..307c5079c 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -118,6 +118,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y # to do this if DMA is enabled to prevent non-DMA-able CCM memory from # being a part of the stack. # +CONFIG_STM32_CCMEXCLUDE=y # # On-board FSMC SRAM configuration @@ -796,16 +797,16 @@ CONFIG_FS_BINFS=y # CONFIG_MMCSD=y -CONFIG_MMCSD_SPI=y +#CONFIG_MMCSD_SPI=y CONFIG_MMCSD_SDIO=y CONFIG_MTD=y # # SPI-based MMC/SD driver # -CONFIG_MMCSD_NSLOTS=1 -CONFIG_MMCSD_READONLY=n -CONFIG_MMCSD_SPICLOCK=12500000 +#CONFIG_MMCSD_NSLOTS=1 +#CONFIG_MMCSD_READONLY=n +#CONFIG_MMCSD_SPICLOCK=12500000 # # STM32 SDIO-based MMC/SD driver -- cgit v1.2.3 From c52278f67942733ac3d462e8a91a01b22b913d40 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Apr 2013 12:35:19 +0200 Subject: Allowed board to init properly as intended with or without SPI2 --- src/drivers/boards/px4fmu/px4fmu_init.c | 59 ++++++++++++++------------------- 1 file changed, 24 insertions(+), 35 deletions(-) diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 96c7fa2df..5dd70c3f9 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -148,16 +148,10 @@ __EXPORT int nsh_archinitialize(void) { int result; -// /* configure ADC pins */ -// stm32_configgpio(GPIO_ADC1_IN10); -// stm32_configgpio(GPIO_ADC1_IN11); -// stm32_configgpio(GPIO_ADC1_IN12); - -// /* configure power supply control pins */ -// stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); -// stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); -// stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); -// stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + /* configure always-on ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + /* IN12 and IN13 further below */ /* configure the high-resolution time/callout interface */ hrt_init(); @@ -200,39 +194,39 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - // Default SPI1 to 1MHz and de-assert the known chip selects. + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi1, 10000000); SPI_SETBITS(spi1, 8); SPI_SETMODE(spi1, SPIDEV_MODE3); SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); -// SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); -// SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\r\n"); - + /* + * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. + * Keep the SPI2 init optional and conditionally initialize the ADC pins + */ spi2 = up_spiinitialize(2); if (!spi2) { - message("[boot] FAILED to initialize SPI port 2\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + } else { + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); + + message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); } - // Default SPI1 to 1MHz and de-assert the known chip selects. - SPI_SETFREQUENCY(spi2, 10000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); -// SPI_SELECT(spi2, PX4_SPIDEV_ACCEL, false); -// SPI_SELECT(spi2, PX4_SPIDEV_MPU, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port2\r\n"); - /* Get the SPI port for the microSD slot */ message("[boot] Initializing SPI port 3\n"); @@ -257,10 +251,5 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); -// stm32_configgpio(GPIO_ADC1_IN12); -// stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - return OK; } -- cgit v1.2.3 From 3ecdca41e504cbf49b03fb543239813886590bd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Apr 2013 12:36:26 +0200 Subject: Cut over attitude estimator to new-style config for all boards --- apps/attitude_estimator_ekf/Makefile | 56 - .../attitude_estimator_ekf_main.c | 465 -------- .../attitude_estimator_ekf_params.c | 105 -- .../attitude_estimator_ekf_params.h | 68 -- .../codegen/attitudeKalmanfilter.c | 1148 -------------------- .../codegen/attitudeKalmanfilter.h | 34 - .../codegen/attitudeKalmanfilter_initialize.c | 31 - .../codegen/attitudeKalmanfilter_initialize.h | 34 - .../codegen/attitudeKalmanfilter_terminate.c | 31 - .../codegen/attitudeKalmanfilter_terminate.h | 34 - .../codegen/attitudeKalmanfilter_types.h | 16 - apps/attitude_estimator_ekf/codegen/cross.c | 37 - apps/attitude_estimator_ekf/codegen/cross.h | 34 - apps/attitude_estimator_ekf/codegen/eye.c | 51 - apps/attitude_estimator_ekf/codegen/eye.h | 35 - apps/attitude_estimator_ekf/codegen/mrdivide.c | 357 ------ apps/attitude_estimator_ekf/codegen/mrdivide.h | 36 - apps/attitude_estimator_ekf/codegen/norm.c | 54 - apps/attitude_estimator_ekf/codegen/norm.h | 34 - apps/attitude_estimator_ekf/codegen/rdivide.c | 38 - apps/attitude_estimator_ekf/codegen/rdivide.h | 34 - apps/attitude_estimator_ekf/codegen/rtGetInf.c | 139 --- apps/attitude_estimator_ekf/codegen/rtGetInf.h | 23 - apps/attitude_estimator_ekf/codegen/rtGetNaN.c | 96 -- apps/attitude_estimator_ekf/codegen/rtGetNaN.h | 21 - apps/attitude_estimator_ekf/codegen/rt_defines.h | 24 - apps/attitude_estimator_ekf/codegen/rt_nonfinite.c | 87 -- apps/attitude_estimator_ekf/codegen/rt_nonfinite.h | 53 - apps/attitude_estimator_ekf/codegen/rtwtypes.h | 159 --- makefiles/config_px4fmu_default.mk | 6 +- makefiles/config_px4fmuv2_default.mk | 6 +- .../attitude_estimator_ekf_main.c | 464 ++++++++ .../attitude_estimator_ekf_params.c | 105 ++ .../attitude_estimator_ekf_params.h | 68 ++ .../codegen/attitudeKalmanfilter.c | 1148 ++++++++++++++++++++ .../codegen/attitudeKalmanfilter.h | 34 + .../codegen/attitudeKalmanfilter_initialize.c | 31 + .../codegen/attitudeKalmanfilter_initialize.h | 34 + .../codegen/attitudeKalmanfilter_terminate.c | 31 + .../codegen/attitudeKalmanfilter_terminate.h | 34 + .../codegen/attitudeKalmanfilter_types.h | 16 + src/modules/attitude_estimator_ekf/codegen/cross.c | 37 + src/modules/attitude_estimator_ekf/codegen/cross.h | 34 + src/modules/attitude_estimator_ekf/codegen/eye.c | 51 + src/modules/attitude_estimator_ekf/codegen/eye.h | 35 + .../attitude_estimator_ekf/codegen/mrdivide.c | 357 ++++++ .../attitude_estimator_ekf/codegen/mrdivide.h | 36 + src/modules/attitude_estimator_ekf/codegen/norm.c | 54 + src/modules/attitude_estimator_ekf/codegen/norm.h | 34 + .../attitude_estimator_ekf/codegen/rdivide.c | 38 + .../attitude_estimator_ekf/codegen/rdivide.h | 34 + .../attitude_estimator_ekf/codegen/rtGetInf.c | 139 +++ .../attitude_estimator_ekf/codegen/rtGetInf.h | 23 + .../attitude_estimator_ekf/codegen/rtGetNaN.c | 96 ++ .../attitude_estimator_ekf/codegen/rtGetNaN.h | 21 + .../attitude_estimator_ekf/codegen/rt_defines.h | 24 + .../attitude_estimator_ekf/codegen/rt_nonfinite.c | 87 ++ .../attitude_estimator_ekf/codegen/rt_nonfinite.h | 53 + .../attitude_estimator_ekf/codegen/rtwtypes.h | 159 +++ src/modules/attitude_estimator_ekf/module.mk | 16 + 60 files changed, 3303 insertions(+), 3336 deletions(-) delete mode 100755 apps/attitude_estimator_ekf/Makefile delete mode 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c delete mode 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c delete mode 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h delete mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h delete mode 100755 apps/attitude_estimator_ekf/codegen/cross.c delete mode 100755 apps/attitude_estimator_ekf/codegen/cross.h delete mode 100755 apps/attitude_estimator_ekf/codegen/eye.c delete mode 100755 apps/attitude_estimator_ekf/codegen/eye.h delete mode 100755 apps/attitude_estimator_ekf/codegen/mrdivide.c delete mode 100755 apps/attitude_estimator_ekf/codegen/mrdivide.h delete mode 100755 apps/attitude_estimator_ekf/codegen/norm.c delete mode 100755 apps/attitude_estimator_ekf/codegen/norm.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rdivide.c delete mode 100755 apps/attitude_estimator_ekf/codegen/rdivide.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.c delete mode 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.c delete mode 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rt_defines.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.c delete mode 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.h delete mode 100755 apps/attitude_estimator_ekf/codegen/rtwtypes.h create mode 100755 src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c create mode 100755 src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c create mode 100755 src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/cross.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/cross.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/eye.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/eye.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/mrdivide.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/mrdivide.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/norm.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/norm.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/rdivide.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/rdivide.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetInf.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetInf.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_defines.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c create mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h create mode 100755 src/modules/attitude_estimator_ekf/codegen/rtwtypes.h create mode 100644 src/modules/attitude_estimator_ekf/module.mk diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile deleted file mode 100755 index 734af7cc1..000000000 --- a/apps/attitude_estimator_ekf/Makefile +++ /dev/null @@ -1,56 +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. -# -############################################################################ - -APPNAME = attitude_estimator_ekf -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -CSRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c - - -# XXX this is *horribly* broken -INCLUDES += $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c deleted file mode 100755 index bd972f03f..000000000 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * 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 attitude_estimator_ekf_main.c - * - * Extended Kalman Filter for Attitude Estimation. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" -#include "attitude_estimator_ekf_params.h" - -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ - -/** - * Mainloop of attitude_estimator_ekf. - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The attitude_estimator_ekf app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int attitude_estimator_ekf_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("attitude_estimator_ekf already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 12400, - attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); - - } else { - printf("\tattitude_estimator_ekf app not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/* - * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - -/* - * EKF Attitude Estimator main function. - * - * Estimates the attitude recursively once started. - * - * @param argc number of commandline arguments (plus command name) - * @param argv strings containing the arguments - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]) -{ - -const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - - float dt = 0.005f; -/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ - float x_aposteriori_k[12]; /**< states */ - float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ - - float x_aposteriori[12]; - float P_aposteriori[144]; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - - int overloadcounter = 19; - - /* Initialize filter */ - attitudeKalmanfilter_initialize(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - - - int loopcounter = 0; - int printcounter = 0; - - thread_running = true; - - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - - /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; - uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - - struct attitude_estimator_ekf_params ekf_params; - - struct attitude_estimator_ekf_param_handles ekf_param_handles; - - /* initialize parameter handles */ - parameters_init(&ekf_param_handles); - - uint64_t start_time = hrt_absolute_time(); - bool initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; - - /* register the perf counter */ - perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); - - /* Main loop*/ - while (!thread_should_exit) { - - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; - int ret = poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); - - if (!state.flag_hil_enabled) { - fprintf(stderr, - "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); - } - - } else { - - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&ekf_param_handles, &ekf_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } - - } else { - - perf_begin(ekf_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { - dt = 0.005f; - parameters_update(&ekf_param_handles, &ekf_params); - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; - } - - uint64_t timing_start = hrt_absolute_time(); - - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); - - /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - continue; - } - - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; - - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; - att.rollacc = x_aposteriori[3]; - att.pitchacc = x_aposteriori[4]; - att.yawacc = x_aposteriori[5]; - - //att.yawspeed =z_k[2] ; - /* copy offsets */ - memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); - att.R_valid = true; - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - - } else { - warnx("NaN in roll/pitch/yaw estimate!"); - } - - perf_end(ekf_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c deleted file mode 100755 index 7d3812abd..000000000 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ /dev/null @@ -1,105 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * 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 attitude_estimator_ekf_params.c - * - * Parameters for EKF filter - */ - -#include "attitude_estimator_ekf_params.h" - -/* Extended Kalman Filter covariances */ - -/* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); -/* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); - -/* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); -/* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); - -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); - -int parameters_init(struct attitude_estimator_ekf_param_handles *h) -{ - /* PID parameters */ - h->q0 = param_find("EKF_ATT_V2_Q0"); - h->q1 = param_find("EKF_ATT_V2_Q1"); - h->q2 = param_find("EKF_ATT_V2_Q2"); - h->q3 = param_find("EKF_ATT_V2_Q3"); - h->q4 = param_find("EKF_ATT_V2_Q4"); - - h->r0 = param_find("EKF_ATT_V2_R0"); - h->r1 = param_find("EKF_ATT_V2_R1"); - h->r2 = param_find("EKF_ATT_V2_R2"); - h->r3 = param_find("EKF_ATT_V2_R3"); - - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) -{ - param_get(h->q0, &(p->q[0])); - param_get(h->q1, &(p->q[1])); - param_get(h->q2, &(p->q[2])); - param_get(h->q3, &(p->q[3])); - param_get(h->q4, &(p->q[4])); - - param_get(h->r0, &(p->r[0])); - param_get(h->r1, &(p->r[1])); - param_get(h->r2, &(p->r[2])); - param_get(h->r3, &(p->r[3])); - - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - - return OK; -} diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h deleted file mode 100755 index 09817d58e..000000000 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * 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 attitude_estimator_ekf_params.h - * - * Parameters for EKF filter - */ - -#include - -struct attitude_estimator_ekf_params { - float r[9]; - float q[12]; - float roll_off; - float pitch_off; - float yaw_off; -}; - -struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3; - param_t q0, q1, q2, q3, q4; - param_t roll_off, pitch_off, yaw_off; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct attitude_estimator_ekf_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c deleted file mode 100755 index 9e97ad11a..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ /dev/null @@ -1,1148 +0,0 @@ -/* - * attitudeKalmanfilter.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" -#include "norm.h" -#include "cross.h" -#include "eye.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) -{ - real32_T y; - int32_T b_u0; - int32_T b_u1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - if (u0 > 0.0F) { - b_u0 = 1; - } else { - b_u0 = -1; - } - - if (u1 > 0.0F) { - b_u1 = 1; - } else { - b_u1 = -1; - } - - y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); - } else if (u1 == 0.0F) { - if (u0 > 0.0F) { - y = RT_PIF / 2.0F; - } else if (u0 < 0.0F) { - y = -(RT_PIF / 2.0F); - } else { - y = 0.0F; - } - } else { - y = (real32_T)atan2(u0, u1); - } - - return y; -} - -/* - * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) - */ -void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const - real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T - P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T - eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) -{ - real32_T wak[3]; - real32_T O[9]; - real_T dv0[9]; - real32_T a[9]; - int32_T i; - real32_T b_a[9]; - real32_T x_n_b[3]; - real32_T b_x_aposteriori_k[3]; - real32_T z_n_b[3]; - real32_T c_a[3]; - real32_T d_a[3]; - int32_T i0; - real32_T x_apriori[12]; - real_T dv1[144]; - real32_T A_lin[144]; - static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T b_A_lin[144]; - real32_T b_q[144]; - real32_T c_A_lin[144]; - real32_T d_A_lin[144]; - real32_T e_A_lin[144]; - int32_T i1; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T K_k[108]; - real32_T fv0[81]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T b_r[81]; - real32_T fv1[81]; - real32_T f0; - real32_T c_P_apriori[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T fv2[36]; - static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T c_r[9]; - real32_T b_K_k[36]; - real32_T d_P_apriori[72]; - static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0 }; - - real32_T c_K_k[72]; - static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0 }; - - real32_T b_z[6]; - static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1 }; - - static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1 }; - - real32_T fv3[6]; - real32_T c_z[6]; - - /* Extended Attitude Kalmanfilter */ - /* */ - /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ - /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ - /* */ - /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ - /* */ - /* Example.... */ - /* */ - /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ - /* coder.varsize('udpIndVect', [9,1], [1,0]) */ - /* udpIndVect=find(updVect); */ - /* process and measurement noise covariance matrix */ - /* Q = diag(q.^2*dt); */ - /* observation matrix */ - /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ - /* % prediction section */ - /* body angular accelerations */ - /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ - wak[0] = x_aposteriori_k[3]; - wak[1] = x_aposteriori_k[4]; - wak[2] = x_aposteriori_k[5]; - - /* body angular rates */ - /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ - /* derivative of the prediction rotation matrix */ - /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ - O[0] = 0.0F; - O[1] = -x_aposteriori_k[2]; - O[2] = x_aposteriori_k[1]; - O[3] = x_aposteriori_k[2]; - O[4] = 0.0F; - O[5] = -x_aposteriori_k[0]; - O[6] = -x_aposteriori_k[1]; - O[7] = x_aposteriori_k[0]; - O[8] = 0.0F; - - /* prediction of the earth z vector */ - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* prediction of the magnetic vector */ - /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - b_a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:66' -zez,0,zex; */ - /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:69' -muz,0,mux; */ - /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:74' E=eye(3); */ - /* 'attitudeKalmanfilter:76' Z=zeros(3); */ - /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ - x_n_b[0] = x_aposteriori_k[0]; - x_n_b[1] = x_aposteriori_k[1]; - x_n_b[2] = x_aposteriori_k[2]; - b_x_aposteriori_k[0] = x_aposteriori_k[6]; - b_x_aposteriori_k[1] = x_aposteriori_k[7]; - b_x_aposteriori_k[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; - for (i = 0; i < 3; i++) { - c_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; - } - - d_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; - } - - x_apriori[i] = x_n_b[i] + dt * wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 3] = wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 6] = c_a[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 9] = d_a[i]; - } - - /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ - /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; - } - - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * i) + 3] = 0.0F; - } - } - - A_lin[6] = 0.0F; - A_lin[7] = x_aposteriori_k[8]; - A_lin[8] = -x_aposteriori_k[7]; - A_lin[18] = -x_aposteriori_k[8]; - A_lin[19] = 0.0F; - A_lin[20] = x_aposteriori_k[6]; - A_lin[30] = x_aposteriori_k[7]; - A_lin[31] = -x_aposteriori_k[6]; - A_lin[32] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - A_lin[9] = 0.0F; - A_lin[10] = x_aposteriori_k[11]; - A_lin[11] = -x_aposteriori_k[10]; - A_lin[21] = -x_aposteriori_k[11]; - A_lin[22] = 0.0F; - A_lin[23] = x_aposteriori_k[9]; - A_lin[33] = x_aposteriori_k[7]; - A_lin[34] = -x_aposteriori_k[9]; - A_lin[35] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * - dt; - } - } - - /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ - /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ - /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ - /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ - /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ - /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - b_q[0] = q[0]; - b_q[12] = 0.0F; - b_q[24] = 0.0F; - b_q[36] = 0.0F; - b_q[48] = 0.0F; - b_q[60] = 0.0F; - b_q[72] = 0.0F; - b_q[84] = 0.0F; - b_q[96] = 0.0F; - b_q[108] = 0.0F; - b_q[120] = 0.0F; - b_q[132] = 0.0F; - b_q[1] = 0.0F; - b_q[13] = q[0]; - b_q[25] = 0.0F; - b_q[37] = 0.0F; - b_q[49] = 0.0F; - b_q[61] = 0.0F; - b_q[73] = 0.0F; - b_q[85] = 0.0F; - b_q[97] = 0.0F; - b_q[109] = 0.0F; - b_q[121] = 0.0F; - b_q[133] = 0.0F; - b_q[2] = 0.0F; - b_q[14] = 0.0F; - b_q[26] = q[0]; - b_q[38] = 0.0F; - b_q[50] = 0.0F; - b_q[62] = 0.0F; - b_q[74] = 0.0F; - b_q[86] = 0.0F; - b_q[98] = 0.0F; - b_q[110] = 0.0F; - b_q[122] = 0.0F; - b_q[134] = 0.0F; - b_q[3] = 0.0F; - b_q[15] = 0.0F; - b_q[27] = 0.0F; - b_q[39] = q[1]; - b_q[51] = 0.0F; - b_q[63] = 0.0F; - b_q[75] = 0.0F; - b_q[87] = 0.0F; - b_q[99] = 0.0F; - b_q[111] = 0.0F; - b_q[123] = 0.0F; - b_q[135] = 0.0F; - b_q[4] = 0.0F; - b_q[16] = 0.0F; - b_q[28] = 0.0F; - b_q[40] = 0.0F; - b_q[52] = q[1]; - b_q[64] = 0.0F; - b_q[76] = 0.0F; - b_q[88] = 0.0F; - b_q[100] = 0.0F; - b_q[112] = 0.0F; - b_q[124] = 0.0F; - b_q[136] = 0.0F; - b_q[5] = 0.0F; - b_q[17] = 0.0F; - b_q[29] = 0.0F; - b_q[41] = 0.0F; - b_q[53] = 0.0F; - b_q[65] = q[1]; - b_q[77] = 0.0F; - b_q[89] = 0.0F; - b_q[101] = 0.0F; - b_q[113] = 0.0F; - b_q[125] = 0.0F; - b_q[137] = 0.0F; - b_q[6] = 0.0F; - b_q[18] = 0.0F; - b_q[30] = 0.0F; - b_q[42] = 0.0F; - b_q[54] = 0.0F; - b_q[66] = 0.0F; - b_q[78] = q[2]; - b_q[90] = 0.0F; - b_q[102] = 0.0F; - b_q[114] = 0.0F; - b_q[126] = 0.0F; - b_q[138] = 0.0F; - b_q[7] = 0.0F; - b_q[19] = 0.0F; - b_q[31] = 0.0F; - b_q[43] = 0.0F; - b_q[55] = 0.0F; - b_q[67] = 0.0F; - b_q[79] = 0.0F; - b_q[91] = q[2]; - b_q[103] = 0.0F; - b_q[115] = 0.0F; - b_q[127] = 0.0F; - b_q[139] = 0.0F; - b_q[8] = 0.0F; - b_q[20] = 0.0F; - b_q[32] = 0.0F; - b_q[44] = 0.0F; - b_q[56] = 0.0F; - b_q[68] = 0.0F; - b_q[80] = 0.0F; - b_q[92] = 0.0F; - b_q[104] = q[2]; - b_q[116] = 0.0F; - b_q[128] = 0.0F; - b_q[140] = 0.0F; - b_q[9] = 0.0F; - b_q[21] = 0.0F; - b_q[33] = 0.0F; - b_q[45] = 0.0F; - b_q[57] = 0.0F; - b_q[69] = 0.0F; - b_q[81] = 0.0F; - b_q[93] = 0.0F; - b_q[105] = 0.0F; - b_q[117] = q[3]; - b_q[129] = 0.0F; - b_q[141] = 0.0F; - b_q[10] = 0.0F; - b_q[22] = 0.0F; - b_q[34] = 0.0F; - b_q[46] = 0.0F; - b_q[58] = 0.0F; - b_q[70] = 0.0F; - b_q[82] = 0.0F; - b_q[94] = 0.0F; - b_q[106] = 0.0F; - b_q[118] = 0.0F; - b_q[130] = q[3]; - b_q[142] = 0.0F; - b_q[11] = 0.0F; - b_q[23] = 0.0F; - b_q[35] = 0.0F; - b_q[47] = 0.0F; - b_q[59] = 0.0F; - b_q[71] = 0.0F; - b_q[83] = 0.0F; - b_q[95] = 0.0F; - b_q[107] = 0.0F; - b_q[119] = 0.0F; - b_q[131] = 0.0F; - b_q[143] = q[3]; - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * - i0]; - } - - c_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 12; i0++) { - d_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - - e_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; - } - } - - /* % update */ - /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:112' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ - /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ - /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* [zw;ze;zmk]; */ - /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 - + 12 * i0]; - } - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } - - b_r[0] = r[0]; - b_r[9] = 0.0F; - b_r[18] = 0.0F; - b_r[27] = 0.0F; - b_r[36] = 0.0F; - b_r[45] = 0.0F; - b_r[54] = 0.0F; - b_r[63] = 0.0F; - b_r[72] = 0.0F; - b_r[1] = 0.0F; - b_r[10] = r[0]; - b_r[19] = 0.0F; - b_r[28] = 0.0F; - b_r[37] = 0.0F; - b_r[46] = 0.0F; - b_r[55] = 0.0F; - b_r[64] = 0.0F; - b_r[73] = 0.0F; - b_r[2] = 0.0F; - b_r[11] = 0.0F; - b_r[20] = r[0]; - b_r[29] = 0.0F; - b_r[38] = 0.0F; - b_r[47] = 0.0F; - b_r[56] = 0.0F; - b_r[65] = 0.0F; - b_r[74] = 0.0F; - b_r[3] = 0.0F; - b_r[12] = 0.0F; - b_r[21] = 0.0F; - b_r[30] = r[1]; - b_r[39] = 0.0F; - b_r[48] = 0.0F; - b_r[57] = 0.0F; - b_r[66] = 0.0F; - b_r[75] = 0.0F; - b_r[4] = 0.0F; - b_r[13] = 0.0F; - b_r[22] = 0.0F; - b_r[31] = 0.0F; - b_r[40] = r[1]; - b_r[49] = 0.0F; - b_r[58] = 0.0F; - b_r[67] = 0.0F; - b_r[76] = 0.0F; - b_r[5] = 0.0F; - b_r[14] = 0.0F; - b_r[23] = 0.0F; - b_r[32] = 0.0F; - b_r[41] = 0.0F; - b_r[50] = r[1]; - b_r[59] = 0.0F; - b_r[68] = 0.0F; - b_r[77] = 0.0F; - b_r[6] = 0.0F; - b_r[15] = 0.0F; - b_r[24] = 0.0F; - b_r[33] = 0.0F; - b_r[42] = 0.0F; - b_r[51] = 0.0F; - b_r[60] = r[2]; - b_r[69] = 0.0F; - b_r[78] = 0.0F; - b_r[7] = 0.0F; - b_r[16] = 0.0F; - b_r[25] = 0.0F; - b_r[34] = 0.0F; - b_r[43] = 0.0F; - b_r[52] = 0.0F; - b_r[61] = 0.0F; - b_r[70] = r[2]; - b_r[79] = 0.0F; - b_r[8] = 0.0F; - b_r[17] = 0.0F; - b_r[26] = 0.0F; - b_r[35] = 0.0F; - b_r[44] = 0.0F; - b_r[53] = 0.0F; - b_r[62] = 0.0F; - b_r[71] = 0.0F; - b_r[80] = r[2]; - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv1, K_k); - - /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - O[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * O[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:138' else */ - /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ - /* 'attitudeKalmanfilter:142' 0,r(1),0; */ - /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - c_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv3[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 12; i0++) { - fv2[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * - i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - O[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; - } - } - } - - c_r[0] = r[0]; - c_r[3] = 0.0F; - c_r[6] = 0.0F; - c_r[1] = 0.0F; - c_r[4] = r[0]; - c_r[7] = 0.0F; - c_r[2] = 0.0F; - c_r[5] = 0.0F; - c_r[8] = r[0]; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; - } - } - - b_mrdivide(c_P_apriori, a, b_K_k); - - /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; - } - - x_n_b[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:156' else */ - /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) - { - /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:159' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ - /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ - /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv5[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[1]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[1]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[1]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 6; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; - } - - b_z[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * b_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 - + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:181' else */ - /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) - { - /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv7[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * - i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[2]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[2]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[2]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - b_z[i] = z[i]; - } - - for (i = 0; i < 3; i++) { - b_z[i + 3] = z[i + 6]; - } - - for (i = 0; i < 6; i++) { - fv3[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; - } - - c_z[i] = b_z[i] - fv3[i]; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * c_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * - P_apriori[i1 + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:202' else */ - /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ - for (i = 0; i < 12; i++) { - x_aposteriori[i] = x_apriori[i]; - } - - /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ - memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); - } - } - } - } - - /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = -x_aposteriori[i + 6]; - } - - rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - - /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), wak); - - /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - cross(z_n_b, x_n_b, wak); - - /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - rdivide(x_n_b, norm(wak), wak); - - /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(wak, z_n_b, x_n_b); - - /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ - for (i = 0; i < 3; i++) { - b_x_aposteriori_k[i] = x_n_b[i]; - } - - rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - - /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - for (i = 0; i < 3; i++) { - Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = wak[i]; - Rot_matrix[6 + i] = z_n_b[i]; - } - - /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ - eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); - eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); -} - -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h deleted file mode 100755 index afa63c1a9..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_H__ -#define __ATTITUDEKALMANFILTER_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); -#endif -/* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c deleted file mode 100755 index 7d620d7fa..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.c - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (attitudeKalmanfilter_initialize.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h deleted file mode 100755 index 8b599be66..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.h - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__ -#define __ATTITUDEKALMANFILTER_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_initialize(void); -#endif -/* End of code generation (attitudeKalmanfilter_initialize.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c deleted file mode 100755 index 7f9727419..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.c - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (attitudeKalmanfilter_terminate.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h deleted file mode 100755 index da84a5024..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.h - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__ -#define __ATTITUDEKALMANFILTER_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_terminate(void); -#endif -/* End of code generation (attitudeKalmanfilter_terminate.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h deleted file mode 100755 index 30fd1e75e..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * attitudeKalmanfilter_types.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TYPES_H__ -#define __ATTITUDEKALMANFILTER_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (attitudeKalmanfilter_types.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c deleted file mode 100755 index 84ada9002..000000000 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * cross.c - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "cross.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) -{ - c[0] = a[1] * b[2] - a[2] * b[1]; - c[1] = a[2] * b[0] - a[0] * b[2]; - c[2] = a[0] * b[1] - a[1] * b[0]; -} - -/* End of code generation (cross.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h deleted file mode 100755 index e727f0684..000000000 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * cross.h - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __CROSS_H__ -#define __CROSS_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); -#endif -/* End of code generation (cross.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c deleted file mode 100755 index b89ab58ef..000000000 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ /dev/null @@ -1,51 +0,0 @@ -/* - * eye.c - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "eye.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_eye(real_T I[144]) -{ - int32_T i; - memset(&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; - } -} - -/* - * - */ -void eye(real_T I[9]) -{ - int32_T i; - memset(&I[0], 0, 9U * sizeof(real_T)); - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1.0; - } -} - -/* End of code generation (eye.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h deleted file mode 100755 index 94fbe7671..000000000 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * eye.h - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __EYE_H__ -#define __EYE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_eye(real_T I[144]); -extern void eye(real_T I[9]); -#endif -/* End of code generation (eye.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c deleted file mode 100755 index a810f22e4..000000000 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ /dev/null @@ -1,357 +0,0 @@ -/* - * mrdivide.c - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) -{ - real32_T b_A[9]; - int32_T rtemp; - int32_T k; - real32_T b_B[36]; - int32_T r1; - int32_T r2; - int32_T r3; - real32_T maxval; - real32_T a21; - real32_T Y[36]; - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * rtemp] = B[rtemp + 3 * k]; - } - } - - for (rtemp = 0; rtemp < 12; rtemp++) { - for (k = 0; k < 3; k++) { - b_B[k + 3 * rtemp] = A[rtemp + 12 * k]; - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(b_A[0]); - a21 = (real32_T)fabs(b_A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(b_A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - b_A[r2] /= b_A[r1]; - b_A[r3] /= b_A[r1]; - b_A[3 + r2] -= b_A[r2] * b_A[3 + r1]; - b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; - b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; - b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; - if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { - rtemp = r2; - r2 = r3; - r3 = rtemp; - } - - b_A[3 + r3] /= b_A[3 + r2]; - b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2]; - for (k = 0; k < 12; k++) { - Y[3 * k] = b_B[r1 + 3 * k]; - Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2]; - Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3 - + r3]; - Y[2 + 3 * k] /= b_A[6 + r3]; - Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1]; - Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2]; - Y[1 + 3 * k] /= b_A[3 + r2]; - Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1]; - Y[3 * k] /= b_A[r1]; - } - - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 12; k++) { - y[k + 12 * rtemp] = Y[rtemp + 3 * k]; - } - } -} - -/* - * - */ -void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) -{ - real32_T b_A[36]; - int8_T ipiv[6]; - int32_T i3; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[72]; - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * i3] = B[i3 + 6 * iy]; - } - - ipiv[i3] = (int8_T)(1 + i3); - } - - for (j = 0; j < 5; j++) { - c = j * 7; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 6 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 6; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 6; - iy += 6; - } - } - - i3 = (c - j) + 6; - for (jy = c + 1; jy + 1 <= i3; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 6; - for (k = 1; k <= 5 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i3 = (iy - j) + 12; - for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 6; - iy += 6; - } - } - - for (i3 = 0; i3 < 12; i3++) { - for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * i3] = A[i3 + 12 * iy]; - } - } - - for (jy = 0; jy < 6; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 6 * j]; - Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; - Y[(ipiv[jy] + 6 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 0; k < 6; k++) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 7; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 5; k > -1; k += -1) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i3] = Y[i3 + 6 * iy]; - } - } -} - -/* - * - */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) -{ - real32_T b_A[81]; - int8_T ipiv[9]; - int32_T i2; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[108]; - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * i2] = B[i2 + 9 * iy]; - } - - ipiv[i2] = (int8_T)(1 + i2); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 9 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; - } - } - - i2 = (c - j) + 9; - for (jy = c + 1; jy + 1 <= i2; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 9; - for (k = 1; k <= 8 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i2 = (iy - j) + 18; - for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 9; - iy += 9; - } - } - - for (i2 = 0; i2 < 12; i2++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * i2] = A[i2 + 12 * iy]; - } - } - - for (jy = 0; jy < 9; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 9 * j]; - Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; - Y[(ipiv[jy] + 9 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 10; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i2] = Y[i2 + 9 * iy]; - } - } -} - -/* End of code generation (mrdivide.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h deleted file mode 100755 index 2d3b0d51f..000000000 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * mrdivide.h - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __MRDIVIDE_H__ -#define __MRDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]); -extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]); -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); -#endif -/* End of code generation (mrdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c deleted file mode 100755 index 0c418cc7b..000000000 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ /dev/null @@ -1,54 +0,0 @@ -/* - * norm.c - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "norm.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -real32_T norm(const real32_T x[3]) -{ - real32_T y; - real32_T scale; - int32_T k; - real32_T absxk; - real32_T t; - y = 0.0F; - scale = 1.17549435E-38F; - for (k = 0; k < 3; k++) { - absxk = (real32_T)fabs(x[k]); - if (absxk > scale) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt(y); -} - -/* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h deleted file mode 100755 index 60cf77b57..000000000 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * norm.h - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __NORM_H__ -#define __NORM_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern real32_T norm(const real32_T x[3]); -#endif -/* End of code generation (norm.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c deleted file mode 100755 index d035dae5e..000000000 --- a/apps/attitude_estimator_ekf/codegen/rdivide.c +++ /dev/null @@ -1,38 +0,0 @@ -/* - * rdivide.c - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) -{ - int32_T i; - for (i = 0; i < 3; i++) { - z[i] = x[i] / y; - } -} - -/* End of code generation (rdivide.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h deleted file mode 100755 index 4bbebebe2..000000000 --- a/apps/attitude_estimator_ekf/codegen/rdivide.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * rdivide.h - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RDIVIDE_H__ -#define __RDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); -#endif -/* End of code generation (rdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c deleted file mode 100755 index 34164d104..000000000 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h deleted file mode 100755 index 145373cd0..000000000 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c deleted file mode 100755 index d84ca9573..000000000 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h deleted file mode 100755 index 65fdaa96f..000000000 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h deleted file mode 100755 index 356498363..000000000 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * rt_defines.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_DEFINES_H__ -#define __RT_DEFINES_H__ - -#include - -#define RT_PI 3.14159265358979323846 -#define RT_PIF 3.1415927F -#define RT_LN_10 2.30258509299404568402 -#define RT_LN_10F 2.3025851F -#define RT_LOG10E 0.43429448190325182765 -#define RT_LOG10EF 0.43429449F -#define RT_E 2.7182818284590452354 -#define RT_EF 2.7182817F -#endif -/* End of code generation (rt_defines.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c deleted file mode 100755 index 303d1d9d2..000000000 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h deleted file mode 100755 index bd56b30d9..000000000 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include -#endif -#include -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h deleted file mode 100755 index 9a5c96267..000000000 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ /dev/null @@ -1,159 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 4dd2a2257..c7109f213 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -16,6 +16,11 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += systemcmds/eeprom +# +# Estimation modules (EKF / other filters) +# +MODULES += modules/attitude_estimator_ekf + # # Transitional support - add commands from the NuttX export archive. # @@ -32,7 +37,6 @@ endef BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ - $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index f86604a73..659b9c95b 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -17,6 +17,11 @@ MODULES += drivers/px4fmu MODULES += drivers/rgbled MODULES += systemcmds/ramtron +# +# Estimation modules (EKF / other filters) +# +MODULES += modules/attitude_estimator_ekf + # # Transitional support - add commands from the NuttX export archive. # @@ -32,7 +37,6 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, attitude_estimator_ekf, , 2048, attitude_estimator_ekf_main) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, boardinfo, , 2048, boardinfo_main ) \ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c new file mode 100755 index 000000000..9fc4dfc83 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -0,0 +1,464 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * 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 attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" + +__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of attitude_estimator_ekf. + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The attitude_estimator_ekf app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int attitude_estimator_ekf_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("attitude_estimator_ekf already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12400, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tattitude_estimator_ekf app is running\n"); + + } else { + printf("\tattitude_estimator_ekf app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]) +{ + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 4); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise attitude */ + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + /* advertise debug value */ + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* initialize parameter handles */ + parameters_init(&ekf_param_handles); + + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + + /* register the perf counter */ + perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2] = { + { .fd = sub_raw, .events = POLLIN }, + { .fd = sub_params, .events = POLLIN } + }; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } + + } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; + + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + + } else { + + perf_begin(ekf_loop_perf); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05f && dt > 0.005f) { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] - ekf_params.yaw_off; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + + //att.yawspeed =z_k[2] ; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + } + + perf_end(ekf_loop_perf); + } + } + } + + loopcounter++; + } + + thread_running = false; + + return 0; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c new file mode 100755 index 000000000..7d3812abd --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * 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 attitude_estimator_ekf_params.c + * + * Parameters for EKF filter + */ + +#include "attitude_estimator_ekf_params.h" + +/* Extended Kalman Filter covariances */ + +/* gyro process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +/* gyro offsets process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); + +/* gyro measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +/* accelerometer measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_V2_Q0"); + h->q1 = param_find("EKF_ATT_V2_Q1"); + h->q2 = param_find("EKF_ATT_V2_Q2"); + h->q3 = param_find("EKF_ATT_V2_Q3"); + h->q4 = param_find("EKF_ATT_V2_Q4"); + + h->r0 = param_find("EKF_ATT_V2_R0"); + h->r1 = param_find("EKF_ATT_V2_R1"); + h->r2 = param_find("EKF_ATT_V2_R2"); + h->r3 = param_find("EKF_ATT_V2_R3"); + + h->roll_off = param_find("ATT_ROLL_OFFS"); + h->pitch_off = param_find("ATT_PITCH_OFFS"); + h->yaw_off = param_find("ATT_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + param_get(h->q0, &(p->q[0])); + param_get(h->q1, &(p->q[1])); + param_get(h->q2, &(p->q[2])); + param_get(h->q3, &(p->q[3])); + param_get(h->q4, &(p->q[4])); + + param_get(h->r0, &(p->r[0])); + param_get(h->r1, &(p->r[1])); + param_get(h->r2, &(p->r[2])); + param_get(h->r3, &(p->r[3])); + + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h new file mode 100755 index 000000000..09817d58e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * 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 attitude_estimator_ekf_params.h + * + * Parameters for EKF filter + */ + +#include + +struct attitude_estimator_ekf_params { + float r[9]; + float q[12]; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_ekf_param_handles { + param_t r0, r1, r2, r3; + param_t q0, q1, q2, q3, q4; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_ekf_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..9e97ad11a --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,1148 @@ +/* + * attitudeKalmanfilter.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "rdivide.h" +#include "norm.h" +#include "cross.h" +#include "eye.h" +#include "mrdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); + +/* Function Definitions */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) +{ + real32_T y; + int32_T b_u0; + int32_T b_u1; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else if (rtIsInfF(u0) && rtIsInfF(u1)) { + if (u0 > 0.0F) { + b_u0 = 1; + } else { + b_u0 = -1; + } + + if (u1 > 0.0F) { + b_u1 = 1; + } else { + b_u1 = -1; + } + + y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); + } else if (u1 == 0.0F) { + if (u0 > 0.0F) { + y = RT_PIF / 2.0F; + } else if (u0 < 0.0F) { + y = -(RT_PIF / 2.0F); + } else { + y = 0.0F; + } + } else { + y = (real32_T)atan2(u0, u1); + } + + return y; +} + +/* + * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) + */ +void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const + real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T + P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T + eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T + P_aposteriori[144]) +{ + real32_T wak[3]; + real32_T O[9]; + real_T dv0[9]; + real32_T a[9]; + int32_T i; + real32_T b_a[9]; + real32_T x_n_b[3]; + real32_T b_x_aposteriori_k[3]; + real32_T z_n_b[3]; + real32_T c_a[3]; + real32_T d_a[3]; + int32_T i0; + real32_T x_apriori[12]; + real_T dv1[144]; + real32_T A_lin[144]; + static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + real32_T b_A_lin[144]; + real32_T b_q[144]; + real32_T c_A_lin[144]; + real32_T d_A_lin[144]; + real32_T e_A_lin[144]; + int32_T i1; + real32_T P_apriori[144]; + real32_T b_P_apriori[108]; + static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + real32_T K_k[108]; + real32_T fv0[81]; + static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + real32_T b_r[81]; + real32_T fv1[81]; + real32_T f0; + real32_T c_P_apriori[36]; + static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + real32_T fv2[36]; + static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + real32_T c_r[9]; + real32_T b_K_k[36]; + real32_T d_P_apriori[72]; + static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0 }; + + real32_T c_K_k[72]; + static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0 }; + + real32_T b_z[6]; + static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1 }; + + static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1 }; + + real32_T fv3[6]; + real32_T c_z[6]; + + /* Extended Attitude Kalmanfilter */ + /* */ + /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ + /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ + /* */ + /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + /* */ + /* Example.... */ + /* */ + /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ + /* coder.varsize('udpIndVect', [9,1], [1,0]) */ + /* udpIndVect=find(updVect); */ + /* process and measurement noise covariance matrix */ + /* Q = diag(q.^2*dt); */ + /* observation matrix */ + /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ + /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ + /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ + /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ + /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ + /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ + /* % prediction section */ + /* body angular accelerations */ + /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ + wak[0] = x_aposteriori_k[3]; + wak[1] = x_aposteriori_k[4]; + wak[2] = x_aposteriori_k[5]; + + /* body angular rates */ + /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_aposteriori_k[2]; + O[2] = x_aposteriori_k[1]; + O[3] = x_aposteriori_k[2]; + O[4] = 0.0F; + O[5] = -x_aposteriori_k[0]; + O[6] = -x_aposteriori_k[1]; + O[7] = x_aposteriori_k[0]; + O[8] = 0.0F; + + /* prediction of the earth z vector */ + /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ + eye(dv0); + for (i = 0; i < 9; i++) { + a[i] = (real32_T)dv0[i] + O[i] * dt; + } + + /* prediction of the magnetic vector */ + /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ + eye(dv0); + for (i = 0; i < 9; i++) { + b_a[i] = (real32_T)dv0[i] + O[i] * dt; + } + + /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ + /* 'attitudeKalmanfilter:66' -zez,0,zex; */ + /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ + /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ + /* 'attitudeKalmanfilter:69' -muz,0,mux; */ + /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ + /* 'attitudeKalmanfilter:74' E=eye(3); */ + /* 'attitudeKalmanfilter:76' Z=zeros(3); */ + /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ + x_n_b[0] = x_aposteriori_k[0]; + x_n_b[1] = x_aposteriori_k[1]; + x_n_b[2] = x_aposteriori_k[2]; + b_x_aposteriori_k[0] = x_aposteriori_k[6]; + b_x_aposteriori_k[1] = x_aposteriori_k[7]; + b_x_aposteriori_k[2] = x_aposteriori_k[8]; + z_n_b[0] = x_aposteriori_k[9]; + z_n_b[1] = x_aposteriori_k[10]; + z_n_b[2] = x_aposteriori_k[11]; + for (i = 0; i < 3; i++) { + c_a[i] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; + } + + d_a[i] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; + } + + x_apriori[i] = x_n_b[i] + dt * wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 6] = c_a[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 9] = d_a[i]; + } + + /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ + /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ + /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ + /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; + } + + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * i) + 3] = 0.0F; + } + } + + A_lin[6] = 0.0F; + A_lin[7] = x_aposteriori_k[8]; + A_lin[8] = -x_aposteriori_k[7]; + A_lin[18] = -x_aposteriori_k[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_aposteriori_k[6]; + A_lin[30] = x_aposteriori_k[7]; + A_lin[31] = -x_aposteriori_k[6]; + A_lin[32] = 0.0F; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; + } + } + + A_lin[9] = 0.0F; + A_lin[10] = x_aposteriori_k[11]; + A_lin[11] = -x_aposteriori_k[10]; + A_lin[21] = -x_aposteriori_k[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_aposteriori_k[9]; + A_lin[33] = x_aposteriori_k[7]; + A_lin[34] = -x_aposteriori_k[9]; + A_lin[35] = 0.0F; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * + dt; + } + } + + /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ + /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ + /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ + /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ + /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ + /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + b_q[0] = q[0]; + b_q[12] = 0.0F; + b_q[24] = 0.0F; + b_q[36] = 0.0F; + b_q[48] = 0.0F; + b_q[60] = 0.0F; + b_q[72] = 0.0F; + b_q[84] = 0.0F; + b_q[96] = 0.0F; + b_q[108] = 0.0F; + b_q[120] = 0.0F; + b_q[132] = 0.0F; + b_q[1] = 0.0F; + b_q[13] = q[0]; + b_q[25] = 0.0F; + b_q[37] = 0.0F; + b_q[49] = 0.0F; + b_q[61] = 0.0F; + b_q[73] = 0.0F; + b_q[85] = 0.0F; + b_q[97] = 0.0F; + b_q[109] = 0.0F; + b_q[121] = 0.0F; + b_q[133] = 0.0F; + b_q[2] = 0.0F; + b_q[14] = 0.0F; + b_q[26] = q[0]; + b_q[38] = 0.0F; + b_q[50] = 0.0F; + b_q[62] = 0.0F; + b_q[74] = 0.0F; + b_q[86] = 0.0F; + b_q[98] = 0.0F; + b_q[110] = 0.0F; + b_q[122] = 0.0F; + b_q[134] = 0.0F; + b_q[3] = 0.0F; + b_q[15] = 0.0F; + b_q[27] = 0.0F; + b_q[39] = q[1]; + b_q[51] = 0.0F; + b_q[63] = 0.0F; + b_q[75] = 0.0F; + b_q[87] = 0.0F; + b_q[99] = 0.0F; + b_q[111] = 0.0F; + b_q[123] = 0.0F; + b_q[135] = 0.0F; + b_q[4] = 0.0F; + b_q[16] = 0.0F; + b_q[28] = 0.0F; + b_q[40] = 0.0F; + b_q[52] = q[1]; + b_q[64] = 0.0F; + b_q[76] = 0.0F; + b_q[88] = 0.0F; + b_q[100] = 0.0F; + b_q[112] = 0.0F; + b_q[124] = 0.0F; + b_q[136] = 0.0F; + b_q[5] = 0.0F; + b_q[17] = 0.0F; + b_q[29] = 0.0F; + b_q[41] = 0.0F; + b_q[53] = 0.0F; + b_q[65] = q[1]; + b_q[77] = 0.0F; + b_q[89] = 0.0F; + b_q[101] = 0.0F; + b_q[113] = 0.0F; + b_q[125] = 0.0F; + b_q[137] = 0.0F; + b_q[6] = 0.0F; + b_q[18] = 0.0F; + b_q[30] = 0.0F; + b_q[42] = 0.0F; + b_q[54] = 0.0F; + b_q[66] = 0.0F; + b_q[78] = q[2]; + b_q[90] = 0.0F; + b_q[102] = 0.0F; + b_q[114] = 0.0F; + b_q[126] = 0.0F; + b_q[138] = 0.0F; + b_q[7] = 0.0F; + b_q[19] = 0.0F; + b_q[31] = 0.0F; + b_q[43] = 0.0F; + b_q[55] = 0.0F; + b_q[67] = 0.0F; + b_q[79] = 0.0F; + b_q[91] = q[2]; + b_q[103] = 0.0F; + b_q[115] = 0.0F; + b_q[127] = 0.0F; + b_q[139] = 0.0F; + b_q[8] = 0.0F; + b_q[20] = 0.0F; + b_q[32] = 0.0F; + b_q[44] = 0.0F; + b_q[56] = 0.0F; + b_q[68] = 0.0F; + b_q[80] = 0.0F; + b_q[92] = 0.0F; + b_q[104] = q[2]; + b_q[116] = 0.0F; + b_q[128] = 0.0F; + b_q[140] = 0.0F; + b_q[9] = 0.0F; + b_q[21] = 0.0F; + b_q[33] = 0.0F; + b_q[45] = 0.0F; + b_q[57] = 0.0F; + b_q[69] = 0.0F; + b_q[81] = 0.0F; + b_q[93] = 0.0F; + b_q[105] = 0.0F; + b_q[117] = q[3]; + b_q[129] = 0.0F; + b_q[141] = 0.0F; + b_q[10] = 0.0F; + b_q[22] = 0.0F; + b_q[34] = 0.0F; + b_q[46] = 0.0F; + b_q[58] = 0.0F; + b_q[70] = 0.0F; + b_q[82] = 0.0F; + b_q[94] = 0.0F; + b_q[106] = 0.0F; + b_q[118] = 0.0F; + b_q[130] = q[3]; + b_q[142] = 0.0F; + b_q[11] = 0.0F; + b_q[23] = 0.0F; + b_q[35] = 0.0F; + b_q[47] = 0.0F; + b_q[59] = 0.0F; + b_q[71] = 0.0F; + b_q[83] = 0.0F; + b_q[95] = 0.0F; + b_q[107] = 0.0F; + b_q[119] = 0.0F; + b_q[131] = 0.0F; + b_q[143] = q[3]; + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * + i0]; + } + + c_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; + } + } + + for (i0 = 0; i0 < 12; i0++) { + d_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + } + + e_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + } + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; + } + } + + /* % update */ + /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ + if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { + /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ + if ((z[5] < 4.0F) || (z[4] > 15.0F)) { + /* 'attitudeKalmanfilter:112' r(2)=10000; */ + r[1] = 10000.0F; + } + + /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ + /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ + /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ + /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ + /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ + /* observation matrix */ + /* [zw;ze;zmk]; */ + /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ + /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 9; i0++) { + b_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + + 12 * i0]; + } + } + } + + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 12; i0++) { + K_k[i + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + } + } + + for (i0 = 0; i0 < 9; i0++) { + fv0[i + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; + } + } + } + + b_r[0] = r[0]; + b_r[9] = 0.0F; + b_r[18] = 0.0F; + b_r[27] = 0.0F; + b_r[36] = 0.0F; + b_r[45] = 0.0F; + b_r[54] = 0.0F; + b_r[63] = 0.0F; + b_r[72] = 0.0F; + b_r[1] = 0.0F; + b_r[10] = r[0]; + b_r[19] = 0.0F; + b_r[28] = 0.0F; + b_r[37] = 0.0F; + b_r[46] = 0.0F; + b_r[55] = 0.0F; + b_r[64] = 0.0F; + b_r[73] = 0.0F; + b_r[2] = 0.0F; + b_r[11] = 0.0F; + b_r[20] = r[0]; + b_r[29] = 0.0F; + b_r[38] = 0.0F; + b_r[47] = 0.0F; + b_r[56] = 0.0F; + b_r[65] = 0.0F; + b_r[74] = 0.0F; + b_r[3] = 0.0F; + b_r[12] = 0.0F; + b_r[21] = 0.0F; + b_r[30] = r[1]; + b_r[39] = 0.0F; + b_r[48] = 0.0F; + b_r[57] = 0.0F; + b_r[66] = 0.0F; + b_r[75] = 0.0F; + b_r[4] = 0.0F; + b_r[13] = 0.0F; + b_r[22] = 0.0F; + b_r[31] = 0.0F; + b_r[40] = r[1]; + b_r[49] = 0.0F; + b_r[58] = 0.0F; + b_r[67] = 0.0F; + b_r[76] = 0.0F; + b_r[5] = 0.0F; + b_r[14] = 0.0F; + b_r[23] = 0.0F; + b_r[32] = 0.0F; + b_r[41] = 0.0F; + b_r[50] = r[1]; + b_r[59] = 0.0F; + b_r[68] = 0.0F; + b_r[77] = 0.0F; + b_r[6] = 0.0F; + b_r[15] = 0.0F; + b_r[24] = 0.0F; + b_r[33] = 0.0F; + b_r[42] = 0.0F; + b_r[51] = 0.0F; + b_r[60] = r[2]; + b_r[69] = 0.0F; + b_r[78] = 0.0F; + b_r[7] = 0.0F; + b_r[16] = 0.0F; + b_r[25] = 0.0F; + b_r[34] = 0.0F; + b_r[43] = 0.0F; + b_r[52] = 0.0F; + b_r[61] = 0.0F; + b_r[70] = r[2]; + b_r[79] = 0.0F; + b_r[8] = 0.0F; + b_r[17] = 0.0F; + b_r[26] = 0.0F; + b_r[35] = 0.0F; + b_r[44] = 0.0F; + b_r[53] = 0.0F; + b_r[62] = 0.0F; + b_r[71] = 0.0F; + b_r[80] = r[2]; + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 9; i0++) { + fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; + } + } + + mrdivide(b_P_apriori, fv1, K_k); + + /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 9; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; + } + + O[i] = z[i] - f0; + } + + for (i = 0; i < 12; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + f0 += K_k[i + 12 * i0] * O[i0]; + } + + x_aposteriori[i] = x_apriori[i] + f0; + } + + /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; + } + + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:138' else */ + /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ + if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { + /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ + /* 'attitudeKalmanfilter:142' 0,r(1),0; */ + /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ + /* observation matrix */ + /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ + /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 3; i0++) { + c_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) + iv3[i1 + 12 * i0]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 12; i0++) { + fv2[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * + i0]; + } + } + + for (i0 = 0; i0 < 3; i0++) { + O[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; + } + } + } + + c_r[0] = r[0]; + c_r[3] = 0.0F; + c_r[6] = 0.0F; + c_r[1] = 0.0F; + c_r[4] = r[0]; + c_r[7] = 0.0F; + c_r[2] = 0.0F; + c_r[5] = 0.0F; + c_r[8] = r[0]; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; + } + } + + b_mrdivide(c_P_apriori, a, b_K_k); + + /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 3; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; + } + + x_n_b[i] = z[i] - f0; + } + + for (i = 0; i < 12; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; + } + + x_aposteriori[i] = x_apriori[i] + f0; + } + + /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; + } + + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:156' else */ + /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ + if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) + { + /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ + if ((z[5] < 4.0F) || (z[4] > 15.0F)) { + /* 'attitudeKalmanfilter:159' r(2)=10000; */ + r[1] = 10000.0F; + } + + /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ + /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ + /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ + /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ + /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ + /* observation matrix */ + /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ + /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 6; i0++) { + d_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) + iv5[i1 + 12 * i0]; + } + } + } + + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 12; i0++) { + c_K_k[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 + * i0]; + } + } + + for (i0 = 0; i0 < 6; i0++) { + fv2[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; + } + } + } + + b_K_k[0] = r[0]; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r[0]; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r[0]; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r[1]; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r[1]; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r[1]; + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; + } + } + + c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); + + /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 6; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + } + + b_z[i] = z[i] - f0; + } + + for (i = 0; i < 12; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 6; i0++) { + f0 += c_K_k[i + 12 * i0] * b_z[i0]; + } + + x_aposteriori[i] = x_apriori[i] + f0; + } + + /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 6; i1++) { + f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; + } + + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:181' else */ + /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ + if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) + { + /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ + /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ + /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ + /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ + /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ + /* observation matrix */ + /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 6; i0++) { + d_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) + iv7[i1 + 12 * i0]; + } + } + } + + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 12; i0++) { + c_K_k[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + + 12 * i0]; + } + } + + for (i0 = 0; i0 < 6; i0++) { + fv2[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * + i0]; + } + } + } + + b_K_k[0] = r[0]; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r[0]; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r[0]; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r[2]; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r[2]; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r[2]; + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; + } + } + + c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); + + /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 3; i++) { + b_z[i] = z[i]; + } + + for (i = 0; i < 3; i++) { + b_z[i + 3] = z[i + 6]; + } + + for (i = 0; i < 6; i++) { + fv3[i] = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; + } + + c_z[i] = b_z[i] - fv3[i]; + } + + for (i = 0; i < 12; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 6; i0++) { + f0 += c_K_k[i + 12 * i0] * c_z[i0]; + } + + x_aposteriori[i] = x_apriori[i] + f0; + } + + /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 6; i1++) { + f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; + } + + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * + P_apriori[i1 + 12 * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:202' else */ + /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ + for (i = 0; i < 12; i++) { + x_aposteriori[i] = x_apriori[i]; + } + + /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ + memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); + } + } + } + } + + /* % euler anglels extraction */ + /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + for (i = 0; i < 3; i++) { + x_n_b[i] = -x_aposteriori[i + 6]; + } + + rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); + + /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ + rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& + x_aposteriori[9]), wak); + + /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ + for (i = 0; i < 3; i++) { + x_n_b[i] = wak[i]; + } + + cross(z_n_b, x_n_b, wak); + + /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ + for (i = 0; i < 3; i++) { + x_n_b[i] = wak[i]; + } + + rdivide(x_n_b, norm(wak), wak); + + /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(wak, z_n_b, x_n_b); + + /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ + for (i = 0; i < 3; i++) { + b_x_aposteriori_k[i] = x_n_b[i]; + } + + rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); + + /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (i = 0; i < 3; i++) { + Rot_matrix[i] = x_n_b[i]; + Rot_matrix[3 + i] = wak[i]; + Rot_matrix[6 + i] = z_n_b[i]; + } + + /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); +} + +/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h new file mode 100755 index 000000000..afa63c1a9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -0,0 +1,34 @@ +/* + * attitudeKalmanfilter.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_H__ +#define __ATTITUDEKALMANFILTER_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +#endif +/* End of code generation (attitudeKalmanfilter.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c new file mode 100755 index 000000000..7d620d7fa --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -0,0 +1,31 @@ +/* + * attitudeKalmanfilter_initialize.c + * + * Code generation for function 'attitudeKalmanfilter_initialize' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "attitudeKalmanfilter_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void attitudeKalmanfilter_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (attitudeKalmanfilter_initialize.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h new file mode 100755 index 000000000..8b599be66 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -0,0 +1,34 @@ +/* + * attitudeKalmanfilter_initialize.h + * + * Code generation for function 'attitudeKalmanfilter_initialize' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__ +#define __ATTITUDEKALMANFILTER_INITIALIZE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void attitudeKalmanfilter_initialize(void); +#endif +/* End of code generation (attitudeKalmanfilter_initialize.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c new file mode 100755 index 000000000..7f9727419 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -0,0 +1,31 @@ +/* + * attitudeKalmanfilter_terminate.c + * + * Code generation for function 'attitudeKalmanfilter_terminate' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "attitudeKalmanfilter_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void attitudeKalmanfilter_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (attitudeKalmanfilter_terminate.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h new file mode 100755 index 000000000..da84a5024 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -0,0 +1,34 @@ +/* + * attitudeKalmanfilter_terminate.h + * + * Code generation for function 'attitudeKalmanfilter_terminate' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__ +#define __ATTITUDEKALMANFILTER_TERMINATE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void attitudeKalmanfilter_terminate(void); +#endif +/* End of code generation (attitudeKalmanfilter_terminate.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h new file mode 100755 index 000000000..30fd1e75e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -0,0 +1,16 @@ +/* + * attitudeKalmanfilter_types.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_TYPES_H__ +#define __ATTITUDEKALMANFILTER_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (attitudeKalmanfilter_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 000000000..84ada9002 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/* + * cross.c + * + * Code generation for function 'cross' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "cross.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) +{ + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; +} + +/* End of code generation (cross.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 000000000..e727f0684 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/* + * cross.h + * + * Code generation for function 'cross' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __CROSS_H__ +#define __CROSS_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); +#endif +/* End of code generation (cross.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c new file mode 100755 index 000000000..b89ab58ef --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.c @@ -0,0 +1,51 @@ +/* + * eye.c + * + * Code generation for function 'eye' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "eye.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void b_eye(real_T I[144]) +{ + int32_T i; + memset(&I[0], 0, 144U * sizeof(real_T)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1.0; + } +} + +/* + * + */ +void eye(real_T I[9]) +{ + int32_T i; + memset(&I[0], 0, 9U * sizeof(real_T)); + for (i = 0; i < 3; i++) { + I[i + 3 * i] = 1.0; + } +} + +/* End of code generation (eye.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h new file mode 100755 index 000000000..94fbe7671 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.h @@ -0,0 +1,35 @@ +/* + * eye.h + * + * Code generation for function 'eye' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __EYE_H__ +#define __EYE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void b_eye(real_T I[144]); +extern void eye(real_T I[9]); +#endif +/* End of code generation (eye.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..a810f22e4 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c @@ -0,0 +1,357 @@ +/* + * mrdivide.c + * + * Code generation for function 'mrdivide' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "mrdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) +{ + real32_T b_A[9]; + int32_T rtemp; + int32_T k; + real32_T b_B[36]; + int32_T r1; + int32_T r2; + int32_T r3; + real32_T maxval; + real32_T a21; + real32_T Y[36]; + for (rtemp = 0; rtemp < 3; rtemp++) { + for (k = 0; k < 3; k++) { + b_A[k + 3 * rtemp] = B[rtemp + 3 * k]; + } + } + + for (rtemp = 0; rtemp < 12; rtemp++) { + for (k = 0; k < 3; k++) { + b_B[k + 3 * rtemp] = A[rtemp + 12 * k]; + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(b_A[0]); + a21 = (real32_T)fabs(b_A[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(b_A[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + b_A[r2] /= b_A[r1]; + b_A[r3] /= b_A[r1]; + b_A[3 + r2] -= b_A[r2] * b_A[3 + r1]; + b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; + b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; + b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; + if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { + rtemp = r2; + r2 = r3; + r3 = rtemp; + } + + b_A[3 + r3] /= b_A[3 + r2]; + b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2]; + for (k = 0; k < 12; k++) { + Y[3 * k] = b_B[r1 + 3 * k]; + Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2]; + Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3 + + r3]; + Y[2 + 3 * k] /= b_A[6 + r3]; + Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1]; + Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2]; + Y[1 + 3 * k] /= b_A[3 + r2]; + Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1]; + Y[3 * k] /= b_A[r1]; + } + + for (rtemp = 0; rtemp < 3; rtemp++) { + for (k = 0; k < 12; k++) { + y[k + 12 * rtemp] = Y[rtemp + 3 * k]; + } + } +} + +/* + * + */ +void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) +{ + real32_T b_A[36]; + int8_T ipiv[6]; + int32_T i3; + int32_T iy; + int32_T j; + int32_T c; + int32_T ix; + real32_T temp; + int32_T k; + real32_T s; + int32_T jy; + int32_T ijA; + real32_T Y[72]; + for (i3 = 0; i3 < 6; i3++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * i3] = B[i3 + 6 * iy]; + } + + ipiv[i3] = (int8_T)(1 + i3); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (int8_T)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 6; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 6; + iy += 6; + } + } + + i3 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i3; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i3 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (i3 = 0; i3 < 12; i3++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * i3] = A[i3 + 12 * iy]; + } + } + + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 0; k < 6; k++) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i3 = 0; i3 < 6; i3++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i3] = Y[i3 + 6 * iy]; + } + } +} + +/* + * + */ +void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) +{ + real32_T b_A[81]; + int8_T ipiv[9]; + int32_T i2; + int32_T iy; + int32_T j; + int32_T c; + int32_T ix; + real32_T temp; + int32_T k; + real32_T s; + int32_T jy; + int32_T ijA; + real32_T Y[108]; + for (i2 = 0; i2 < 9; i2++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * i2] = B[i2 + 9 * iy]; + } + + ipiv[i2] = (int8_T)(1 + i2); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (int8_T)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } + + i2 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i2; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i2 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + for (i2 = 0; i2 < 12; i2++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * i2] = A[i2 + 12 * iy]; + } + } + + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i2 = 0; i2 < 9; i2++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i2] = Y[i2 + 9 * iy]; + } + } +} + +/* End of code generation (mrdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h new file mode 100755 index 000000000..2d3b0d51f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h @@ -0,0 +1,36 @@ +/* + * mrdivide.h + * + * Code generation for function 'mrdivide' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __MRDIVIDE_H__ +#define __MRDIVIDE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]); +extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]); +extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); +#endif +/* End of code generation (mrdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c new file mode 100755 index 000000000..0c418cc7b --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.c @@ -0,0 +1,54 @@ +/* + * norm.c + * + * Code generation for function 'norm' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "norm.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +real32_T norm(const real32_T x[3]) +{ + real32_T y; + real32_T scale; + int32_T k; + real32_T absxk; + real32_T t; + y = 0.0F; + scale = 1.17549435E-38F; + for (k = 0; k < 3; k++) { + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + return scale * (real32_T)sqrt(y); +} + +/* End of code generation (norm.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h new file mode 100755 index 000000000..60cf77b57 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.h @@ -0,0 +1,34 @@ +/* + * norm.h + * + * Code generation for function 'norm' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __NORM_H__ +#define __NORM_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern real32_T norm(const real32_T x[3]); +#endif +/* End of code generation (norm.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c new file mode 100755 index 000000000..d035dae5e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c @@ -0,0 +1,38 @@ +/* + * rdivide.c + * + * Code generation for function 'rdivide' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "rdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) +{ + int32_T i; + for (i = 0; i < 3; i++) { + z[i] = x[i] / y; + } +} + +/* End of code generation (rdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h new file mode 100755 index 000000000..4bbebebe2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h @@ -0,0 +1,34 @@ +/* + * rdivide.h + * + * Code generation for function 'rdivide' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __RDIVIDE_H__ +#define __RDIVIDE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); +#endif +/* End of code generation (rdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c new file mode 100755 index 000000000..34164d104 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * rtGetInf.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, Inf and MinusInf + */ +#include "rtGetInf.h" +#define NumBitsPerChar 8U + +/* Function: rtGetInf ================================================== + * Abstract: + * Initialize rtInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + } + } + + return inf; +} + +/* Function: rtGetInfF ================================================== + * Abstract: + * Initialize rtInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* Function: rtGetMinusInf ================================================== + * Abstract: + * Initialize rtMinusInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetMinusInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + } + } + + return minf; +} + +/* Function: rtGetMinusInfF ================================================== + * Abstract: + * Initialize rtMinusInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h new file mode 100755 index 000000000..145373cd0 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * rtGetInf.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __RTGETINF_H__ +#define __RTGETINF_H__ + +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetInf(void); +extern real32_T rtGetInfF(void); +extern real_T rtGetMinusInf(void); +extern real32_T rtGetMinusInfF(void); + +#endif +/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c new file mode 100755 index 000000000..d84ca9573 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * rtGetNaN.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, NaN + */ +#include "rtGetNaN.h" +#define NumBitsPerChar 8U + +/* Function: rtGetNaN ================================================== + * Abstract: + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetNaN(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; + tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; + nan = tmpVal.fltVal; + break; + } + } + } + + return nan; +} + +/* Function: rtGetNaNF ================================================== + * Abstract: + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0 } }; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + nanF.wordL.wordLuint = 0xFFC00000U; + break; + } + + case BigEndian: + { + nanF.wordL.wordLuint = 0x7FFFFFFFU; + break; + } + } + + return nanF.wordL.wordLreal; +} + +/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h new file mode 100755 index 000000000..65fdaa96f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * rtGetNaN.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __RTGETNAN_H__ +#define __RTGETNAN_H__ + +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetNaN(void); +extern real32_T rtGetNaNF(void); + +#endif +/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 000000000..356498363 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/* + * rt_defines.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __RT_DEFINES_H__ +#define __RT_DEFINES_H__ + +#include + +#define RT_PI 3.14159265358979323846 +#define RT_PIF 3.1415927F +#define RT_LN_10 2.30258509299404568402 +#define RT_LN_10F 2.3025851F +#define RT_LOG10E 0.43429448190325182765 +#define RT_LOG10EF 0.43429449F +#define RT_E 2.7182818284590452354 +#define RT_EF 2.7182817F +#endif +/* End of code generation (rt_defines.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c new file mode 100755 index 000000000..303d1d9d2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * rt_nonfinite.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finites, + * (Inf, NaN and -Inf). + */ +#include "rt_nonfinite.h" +#include "rtGetNaN.h" +#include "rtGetInf.h" + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +/* Function: rt_InitInfAndNaN ================================================== + * Abstract: + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +void rt_InitInfAndNaN(size_t realSize) +{ + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +/* Function: rtIsInf ================================================== + * Abstract: + * Test if value is infinite + */ +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +/* Function: rtIsInfF ================================================= + * Abstract: + * Test if single-precision value is infinite + */ +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +/* Function: rtIsNaN ================================================== + * Abstract: + * Test if value is not a number + */ +boolean_T rtIsNaN(real_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan(value)? TRUE:FALSE; +#else + return (value!=value)? 1U:0U; +#endif +} + +/* Function: rtIsNaNF ================================================= + * Abstract: + * Test if single-precision value is not a number + */ +boolean_T rtIsNaNF(real32_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan((real_T)value)? true:false; +#else + return (value!=value)? 1U:0U; +#endif +} + + +/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h new file mode 100755 index 000000000..bd56b30d9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * rt_nonfinite.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __RT_NONFINITE_H__ +#define __RT_NONFINITE_H__ + +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include +#endif +#include +#include "rtwtypes.h" + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +extern void rt_InitInfAndNaN(size_t realSize); +extern boolean_T rtIsInf(real_T value); +extern boolean_T rtIsInfF(real32_T value); +extern boolean_T rtIsNaN(real_T value); +extern boolean_T rtIsNaNF(real32_T value); + +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +#endif +/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h new file mode 100755 index 000000000..9a5c96267 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/* + * rtwtypes.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Sat Jan 19 15:25:29 2013 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: Generic->MATLAB Host Computer + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk new file mode 100644 index 000000000..4033617c4 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -0,0 +1,16 @@ + +MODULE_NAME = attitude_estimator_ekf +SRCS = attitude_estimator_ekf_main.c \ + attitude_estimator_ekf_params.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/attitudeKalmanfilter.c \ + codegen/cross.c \ + codegen/eye.c \ + codegen/mrdivide.c \ + codegen/norm.c \ + codegen/rdivide.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c + -- cgit v1.2.3 From 0c6eaae8632f3a6520afdaf60adf13a8b6e55566 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 24 Apr 2013 18:42:34 +0400 Subject: Estimate accelerometers offset in position_estimator_inav --- .../position_estimator_inav_main.c | 74 ++++++++++++---------- .../position_estimator_inav_params.c | 8 +++ .../position_estimator_inav_params.h | 3 + 3 files changed, 53 insertions(+), 32 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 97d669612..6e8c0ab5f 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -280,6 +280,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ @@ -471,36 +472,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime t = hrt_absolute_time(); float dt = (t - last_time) / 1000000.0; last_time = t; - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * accel_corr[j]; + if (att.R_valid) { + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, + params.acc_T, params.acc_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); + } + } + accel_NED[2] += const_earth_gravity; + /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* the inverse of a rotation matrix is its transpose, just swap i and j */ + accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + } + } + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { + z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, params.k, + use_z); } - } - accel_NED[2] += const_earth_gravity; - /* kalman filter prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; - if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; - } - if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); } if (verbose_mode) { /* print updates rate */ @@ -521,9 +531,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; + pos.x = accel_offs_est[0]; + pos.vx = accel_offs_est[1]; + pos.y = accel_offs_est[2]; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index d3a165947..8cd9844c7 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,6 +39,7 @@ */ #include "position_estimator_inav_params.h" +#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -51,6 +52,8 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); + PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); @@ -75,6 +78,8 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); @@ -101,6 +106,8 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->acc_offs_w, &(p->acc_offs_w)); + /* read int32 and cast to int16 */ int32_t t; param_get(h->acc_offs_0, &t); @@ -119,5 +126,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->acc_t_20, &(p->acc_T[2][0])); param_get(h->acc_t_21, &(p->acc_T[2][1])); param_get(h->acc_t_22, &(p->acc_T[2][2])); + return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 51e57a914..2c6fb3df2 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,6 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; + float acc_offs_w; int16_t acc_offs[3]; float acc_T[3][3]; }; @@ -57,6 +58,8 @@ struct position_estimator_inav_param_handles { param_t k_alt_20; param_t k_alt_21; + param_t acc_offs_w; + param_t acc_offs_0; param_t acc_offs_1; param_t acc_offs_2; -- cgit v1.2.3 From 98a2445065383015dfe2134a384ebdf3fb7dfc26 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 24 Apr 2013 17:45:08 -0700 Subject: Include the correct Make.defs file --- nuttx/configs/px4fmuv2/nsh/Make.defs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs index 3e6f88bd3..3306e4bf1 100644 --- a/nuttx/configs/px4fmuv2/nsh/Make.defs +++ b/nuttx/configs/px4fmuv2/nsh/Make.defs @@ -1,3 +1,3 @@ include ${TOPDIR}/.config -include $(TOPDIR)/configs/px4fmu/common/Make.defs +include $(TOPDIR)/configs/px4fmuv2/common/Make.defs -- cgit v1.2.3 From aa85fba9796a7eda6874a2719e0bab7cfa2897ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 11:01:47 +0200 Subject: Ported AR.Drone interface to new style config --- apps/ardrone_interface/Makefile | 42 -- apps/ardrone_interface/ardrone_interface.c | 398 ----------------- apps/ardrone_interface/ardrone_motor_control.c | 492 --------------------- apps/ardrone_interface/ardrone_motor_control.h | 93 ---- makefiles/config_px4fmu_default.mk | 2 +- src/drivers/ardrone_interface/ardrone_interface.c | 398 +++++++++++++++++ .../ardrone_interface/ardrone_motor_control.c | 492 +++++++++++++++++++++ .../ardrone_interface/ardrone_motor_control.h | 93 ++++ src/drivers/ardrone_interface/module.mk | 40 ++ 9 files changed, 1024 insertions(+), 1026 deletions(-) delete mode 100644 apps/ardrone_interface/Makefile delete mode 100644 apps/ardrone_interface/ardrone_interface.c delete mode 100644 apps/ardrone_interface/ardrone_motor_control.c delete mode 100644 apps/ardrone_interface/ardrone_motor_control.h create mode 100644 src/drivers/ardrone_interface/ardrone_interface.c create mode 100644 src/drivers/ardrone_interface/ardrone_motor_control.c create mode 100644 src/drivers/ardrone_interface/ardrone_motor_control.h create mode 100644 src/drivers/ardrone_interface/module.mk diff --git a/apps/ardrone_interface/Makefile b/apps/ardrone_interface/Makefile deleted file mode 100644 index fea96082f..000000000 --- a/apps/ardrone_interface/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. -# -############################################################################ - -# -# Makefile to build ardrone interface -# - -APPNAME = ardrone_interface -PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c deleted file mode 100644 index aeaf830de..000000000 --- a/apps/ardrone_interface/ardrone_interface.c +++ /dev/null @@ -1,398 +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 ardrone_interface.c - * Implementation of AR.Drone 1.0 / 2.0 motor control interface. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "ardrone_motor_control.h" - -__EXPORT int ardrone_interface_main(int argc, char *argv[]); - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int ardrone_interface_task; /**< Handle of deamon task / thread */ -static int ardrone_write; /**< UART to write AR.Drone commands to */ - -/** - * Mainloop of ardrone_interface. - */ -int ardrone_interface_thread_main(int argc, char *argv[]); - -/** - * Open the UART connected to the motor controllers - */ -static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int ardrone_interface_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("ardrone_interface already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - ardrone_interface_task = task_spawn("ardrone_interface", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - ardrone_interface_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tardrone_interface is running\n"); - } else { - printf("\tardrone_interface not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original) -{ - /* baud rate */ - int speed = B115200; - int uart; - - /* open uart */ - uart = open(uart_name, O_RDWR | O_NOCTTY); - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] 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, "[ardrone_interface] 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, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - return uart; -} - -int ardrone_interface_thread_main(int argc, char *argv[]) -{ - thread_running = true; - - char *device = "/dev/ttyS1"; - - /* welcome user */ - printf("[ardrone_interface] Control started, taking over motors\n"); - - /* File descriptors */ - int gpios; - - char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n"; - - bool motor_test_mode = false; - int test_motor = -1; - - /* read commandline arguments */ - for (int i = 0; i < argc && argv[i]; i++) { - if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { - motor_test_mode = true; - } - - if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { - if (i+1 < argc) { - int motor = atoi(argv[i+1]); - if (motor > 0 && motor < 5) { - test_motor = motor; - } else { - thread_running = false; - errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); - } - } else { - thread_running = false; - errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); - } - } - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set - if (argc > i + 1) { - device = argv[i + 1]; - - } else { - thread_running = false; - errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); - } - } - } - - struct termios uart_config_original; - - if (motor_test_mode) { - printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); - } - - /* Led animation */ - int counter = 0; - int led_counter = 0; - - /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_armed_s armed; - armed.armed = false; - - /* subscribe to attitude, motor setpoints and system state */ - int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - printf("[ardrone_interface] Motors initialized - ready.\n"); - fflush(stdout); - - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - ardrone_write = ardrone_open_uart(device, &uart_config_original); - - /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ - gpios = ar_multiplexing_init(); - - if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); - thread_running = false; - exit(ERROR); - } - - /* initialize motors */ - if (OK != ar_init_motors(ardrone_write, gpios)) { - close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); - thread_running = false; - exit(ERROR); - } - - ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); - - - // XXX Re-done initialization to make sure it is accepted by the motors - // XXX should be removed after more testing, but no harm - - /* close uarts */ - close(ardrone_write); - - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - ardrone_write = ardrone_open_uart(device, &uart_config_original); - - /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ - gpios = ar_multiplexing_init(); - - if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); - thread_running = false; - exit(ERROR); - } - - /* initialize motors */ - if (OK != ar_init_motors(ardrone_write, gpios)) { - close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); - thread_running = false; - exit(ERROR); - } - - while (!thread_should_exit) { - - if (motor_test_mode) { - /* set motors to idle speed */ - if (test_motor > 0 && test_motor < 5) { - int motors[4] = {0, 0, 0, 0}; - motors[test_motor - 1] = 10; - ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); - } else { - ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); - } - - } else { - /* MAIN OPERATION MODE */ - - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of the actuator controls */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - /* for now only spin if armed and immediately shut down - * if in failsafe - */ - if (armed.armed && !armed.lockdown) { - ardrone_mixing_and_output(ardrone_write, &actuator_controls); - - } else { - /* Silently lock down motor speeds to zero */ - ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); - } - } - - if (counter % 24 == 0) { - if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); - - if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); - - if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); - - if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); - - if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); - - if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); - - if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); - - if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); - - if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); - - if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); - - if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); - - if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); - - led_counter++; - - if (led_counter == 12) led_counter = 0; - } - - /* run at approximately 200 Hz */ - usleep(4500); - - counter++; - } - - /* restore old UART config */ - int termios_state; - - if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); - } - - printf("[ardrone_interface] Restored original UART config, exiting..\n"); - - /* close uarts */ - close(ardrone_write); - ar_multiplexing_deinit(gpios); - - fflush(stdout); - - thread_running = false; - - return OK; -} - diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c deleted file mode 100644 index f15c74a22..000000000 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ /dev/null @@ -1,492 +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 ardrone_motor_control.c - * Implementation of AR.Drone 1.0 / 2.0 motor control interface - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ardrone_motor_control.h" - -static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; -static unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; - -typedef union { - uint16_t motor_value; - uint8_t bytes[2]; -} motor_union_t; - -#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */ - -/** - * @brief Generate the 8-byte motor set packet - * - * @return the number of bytes (8) - */ -void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) -{ - motor_buf[0] = 0x20; - motor_buf[1] = 0x00; - motor_buf[2] = 0x00; - motor_buf[3] = 0x00; - motor_buf[4] = 0x00; - /* - * {0x20, 0x00, 0x00, 0x00, 0x00}; - * 0x20 is start sign / motor command - */ - motor_union_t curr_motor; - uint16_t nineBitMask = 0x1FF; - - /* Set motor 1 */ - curr_motor.motor_value = (motor1 & nineBitMask) << 4; - motor_buf[0] |= curr_motor.bytes[1]; - motor_buf[1] |= curr_motor.bytes[0]; - - /* Set motor 2 */ - curr_motor.motor_value = (motor2 & nineBitMask) << 3; - motor_buf[1] |= curr_motor.bytes[1]; - motor_buf[2] |= curr_motor.bytes[0]; - - /* Set motor 3 */ - curr_motor.motor_value = (motor3 & nineBitMask) << 2; - motor_buf[2] |= curr_motor.bytes[1]; - motor_buf[3] |= curr_motor.bytes[0]; - - /* Set motor 4 */ - curr_motor.motor_value = (motor4 & nineBitMask) << 1; - motor_buf[3] |= curr_motor.bytes[1]; - motor_buf[4] |= curr_motor.bytes[0]; -} - -void ar_enable_broadcast(int fd) -{ - ar_select_motor(fd, 0); -} - -int ar_multiplexing_init() -{ - int fd; - - fd = open(GPIO_DEVICE_PATH, 0); - - if (fd < 0) { - warn("GPIO: open fail"); - return fd; - } - - /* deactivate all outputs */ - if (ioctl(fd, GPIO_SET, motor_gpios)) { - warn("GPIO: clearing pins fail"); - close(fd); - return -1; - } - - /* configure all motor select GPIOs as outputs */ - if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { - warn("GPIO: output set fail"); - close(fd); - return -1; - } - - return fd; -} - -int ar_multiplexing_deinit(int fd) -{ - if (fd < 0) { - printf("GPIO: no valid descriptor\n"); - return fd; - } - - int ret = 0; - - /* deselect motor 1-4 */ - ret += ioctl(fd, GPIO_SET, motor_gpios); - - if (ret != 0) { - printf("GPIO: clear failed %d times\n", ret); - } - - if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) { - printf("GPIO: input set fail\n"); - return -1; - } - - close(fd); - - return ret; -} - -int ar_select_motor(int fd, uint8_t motor) -{ - int ret = 0; - /* - * Four GPIOS: - * GPIO_EXT1 - * GPIO_EXT2 - * GPIO_UART2_CTS - * GPIO_UART2_RTS - */ - - /* select motor 0 to enable broadcast */ - if (motor == 0) { - /* select motor 1-4 */ - ret += ioctl(fd, GPIO_CLEAR, motor_gpios); - - } else { - /* select reqested motor */ - ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); - } - - return ret; -} - -int ar_deselect_motor(int fd, uint8_t motor) -{ - int ret = 0; - /* - * Four GPIOS: - * GPIO_EXT1 - * GPIO_EXT2 - * GPIO_UART2_CTS - * GPIO_UART2_RTS - */ - - if (motor == 0) { - /* deselect motor 1-4 */ - ret += ioctl(fd, GPIO_SET, motor_gpios); - - } else { - /* deselect reqested motor */ - ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); - } - - return ret; -} - -int ar_init_motors(int ardrone_uart, int gpios) -{ - /* Write ARDrone commands on UART2 */ - uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; - uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; - - /* deselect all motors */ - ar_deselect_motor(gpios, 0); - - /* initialize all motors - * - select one motor at a time - * - configure motor - */ - int i; - int errcounter = 0; - - - /* initial setup run */ - for (i = 1; i < 5; ++i) { - /* Initialize motors 1-4 */ - errcounter += ar_select_motor(gpios, i); - usleep(200); - - /* - * write 0xE0 - request status - * receive one status byte - */ - write(ardrone_uart, &(initbuf[0]), 1); - fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); - - /* - * write 0x91 - request checksum - * receive 120 status bytes - */ - write(ardrone_uart, &(initbuf[1]), 1); - fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*120); - - /* - * write 0xA1 - set status OK - * receive one status byte - should be A0 - * to confirm status is OK - */ - write(ardrone_uart, &(initbuf[2]), 1); - fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); - - /* - * set as motor i, where i = 1..4 - * receive nothing - */ - initbuf[3] = (uint8_t)i; - write(ardrone_uart, &(initbuf[3]), 1); - fsync(ardrone_uart); - - /* - * write 0x40 - check version - * receive 11 bytes encoding the version - */ - write(ardrone_uart, &(initbuf[4]), 1); - fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*11); - - ar_deselect_motor(gpios, i); - /* sleep 200 ms */ - usleep(200000); - } - - /* start the multicast part */ - errcounter += ar_select_motor(gpios, 0); - usleep(200); - - /* - * first round - * write six times A0 - enable broadcast - * receive nothing - */ - write(ardrone_uart, multicastbuf, sizeof(multicastbuf)); - fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf)); - - /* - * second round - * write six times A0 - enable broadcast - * receive nothing - */ - write(ardrone_uart, multicastbuf, sizeof(multicastbuf)); - fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf)); - - /* set motors to zero speed (fsync is part of the write command */ - ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); - - if (errcounter != 0) { - fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); - fflush(stdout); - } - return errcounter; -} - -/** - * Sets the leds on the motor controllers, 1 turns led on, 0 off. - */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) -{ - /* - * 2 bytes are sent. The first 3 bits describe the command: 011 means led control - * the following 4 bits are the red leds for motor 4, 3, 2, 1 - * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1 - * the last bit is unknown. - * - * The packet is therefore: - * 011 rrrr 0000 gggg 0 - */ - uint8_t leds[2]; - leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); - leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); - write(ardrone_uart, leds, 2); -} - -int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { - const unsigned int min_motor_interval = 4900; - static uint64_t last_motor_time = 0; - - static struct actuator_outputs_s outputs; - outputs.timestamp = hrt_absolute_time(); - outputs.output[0] = motor1; - outputs.output[1] = motor2; - outputs.output[2] = motor3; - outputs.output[3] = motor4; - static orb_advert_t pub = 0; - if (pub == 0) { - pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); - } - - if (hrt_absolute_time() - last_motor_time > min_motor_interval) { - uint8_t buf[5] = {0}; - ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); - int ret; - ret = write(ardrone_fd, buf, sizeof(buf)); - fsync(ardrone_fd); - - /* publish just written values */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs); - - if (ret == sizeof(buf)) { - return OK; - } else { - return ret; - } - } else { - return -ERROR; - } -} - -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) { - - float roll_control = actuators->control[0]; - float pitch_control = actuators->control[1]; - float yaw_control = actuators->control[2]; - float motor_thrust = actuators->control[3]; - - //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); - - const float min_thrust = 0.02f; /**< 2% minimum thrust */ - const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ - const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ - const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ - - /* initialize all fields to zero */ - uint16_t motor_pwm[4] = {0}; - float motor_calc[4] = {0}; - - float output_band = 0.0f; - float band_factor = 0.75f; - const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - float yaw_factor = 1.0f; - - static bool initialized = false; - /* publish effective outputs */ - static struct actuator_controls_effective_s actuator_controls_effective; - static orb_advert_t actuator_controls_effective_pub; - - if (motor_thrust <= min_thrust) { - motor_thrust = min_thrust; - output_band = 0.0f; - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { - output_band = band_factor * (motor_thrust - min_thrust); - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * startpoint_full_control; - } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * (max_thrust - motor_thrust); - } - - //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer - - // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - - // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - - // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - - // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); - - // if we are not in the output band - if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band - && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band - && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band - && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { - - yaw_factor = 0.5f; - yaw_control *= yaw_factor; - // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - - // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - - // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - - // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); - } - - for (int i = 0; i < 4; i++) { - //check for limits - if (motor_calc[i] < motor_thrust - output_band) { - motor_calc[i] = motor_thrust - output_band; - } - - if (motor_calc[i] > motor_thrust + output_band) { - motor_calc[i] = motor_thrust + output_band; - } - } - - /* publish effective outputs */ - actuator_controls_effective.control_effective[0] = roll_control; - actuator_controls_effective.control_effective[1] = pitch_control; - /* yaw output after limiting */ - actuator_controls_effective.control_effective[2] = yaw_control; - /* possible motor thrust limiting */ - actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; - - if (!initialized) { - /* advertise and publish */ - actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); - initialized = true; - } else { - /* already initialized, just publishing */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); - } - - /* set the motor values */ - - /* scale up from 0..1 to 10..512) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); - - /* Keep motors spinning while armed and prevent overflows */ - - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; - motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; - motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; - motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; - - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; - motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; - motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; - motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; - - /* send motors via UART */ - ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); -} diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h deleted file mode 100644 index 78b603b63..000000000 --- a/apps/ardrone_interface/ardrone_motor_control.h +++ /dev/null @@ -1,93 +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 ardrone_motor_control.h - * Definition of AR.Drone 1.0 / 2.0 motor control interface - */ - -#include -#include - -/** - * Generate the 5-byte motor set packet. - * - * @return the number of bytes (5) - */ -void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); - -/** - * Select a motor in the multiplexing. - * - * @param fd GPIO file descriptor - * @param motor Motor number, from 1 to 4, 0 selects all - */ -int ar_select_motor(int fd, uint8_t motor); - -/** - * Deselect a motor in the multiplexing. - * - * @param fd GPIO file descriptor - * @param motor Motor number, from 1 to 4, 0 deselects all - */ -int ar_deselect_motor(int fd, uint8_t motor); - -void ar_enable_broadcast(int fd); - -int ar_multiplexing_init(void); -int ar_multiplexing_deinit(int fd); - -/** - * Write four motor commands to an already initialized port. - * - * Writing 0 stops a motor, values from 1-512 encode the full thrust range. - * on some motor controller firmware revisions a minimum value of 10 is - * required to spin the motors. - */ -int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); - -/** - * Initialize the motors. - */ -int ar_init_motors(int ardrone_uart, int gpio); - -/** - * Set LED pattern. - */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); - -/** - * Mix motors and output actuators - */ -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators); diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index c7109f213..1302d1582 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -14,6 +14,7 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu MODULES += drivers/lsm303d MODULES += drivers/l3gd20 +MODULES += drivers/ardrone_interface MODULES += systemcmds/eeprom # @@ -36,7 +37,6 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, ardrone_interface, SCHED_PRIORITY_MAX-15, 2048, ardrone_interface_main ) \ $(call _B, bl_update, , 4096, bl_update_main ) \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c new file mode 100644 index 000000000..aeaf830de --- /dev/null +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -0,0 +1,398 @@ +/**************************************************************************** + * + * 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 ardrone_interface.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "ardrone_motor_control.h" + +__EXPORT int ardrone_interface_main(int argc, char *argv[]); + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int ardrone_interface_task; /**< Handle of deamon task / thread */ +static int ardrone_write; /**< UART to write AR.Drone commands to */ + +/** + * Mainloop of ardrone_interface. + */ +int ardrone_interface_thread_main(int argc, char *argv[]); + +/** + * Open the UART connected to the motor controllers + */ +static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int ardrone_interface_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("ardrone_interface already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + ardrone_interface_task = task_spawn("ardrone_interface", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + ardrone_interface_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tardrone_interface is running\n"); + } else { + printf("\tardrone_interface not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original) +{ + /* baud rate */ + int speed = B115200; + int uart; + + /* open uart */ + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + fprintf(stderr, "[ardrone_interface] 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, "[ardrone_interface] 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, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + return uart; +} + +int ardrone_interface_thread_main(int argc, char *argv[]) +{ + thread_running = true; + + char *device = "/dev/ttyS1"; + + /* welcome user */ + printf("[ardrone_interface] Control started, taking over motors\n"); + + /* File descriptors */ + int gpios; + + char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n"; + + bool motor_test_mode = false; + int test_motor = -1; + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { + motor_test_mode = true; + } + + if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { + if (i+1 < argc) { + int motor = atoi(argv[i+1]); + if (motor > 0 && motor < 5) { + test_motor = motor; + } else { + thread_running = false; + errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); + } + } else { + thread_running = false; + errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); + } + } + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); + } + } + } + + struct termios uart_config_original; + + if (motor_test_mode) { + printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); + } + + /* Led animation */ + int counter = 0; + int led_counter = 0; + + /* declare and safely initialize all structs */ + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + struct actuator_armed_s armed; + armed.armed = false; + + /* subscribe to attitude, motor setpoints and system state */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + printf("[ardrone_interface] Motors initialized - ready.\n"); + fflush(stdout); + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + ardrone_write = ardrone_open_uart(device, &uart_config_original); + + /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ + gpios = ar_multiplexing_init(); + + if (ardrone_write < 0) { + fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + thread_running = false; + exit(ERROR); + } + + /* initialize motors */ + if (OK != ar_init_motors(ardrone_write, gpios)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + thread_running = false; + exit(ERROR); + } + + ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); + + + // XXX Re-done initialization to make sure it is accepted by the motors + // XXX should be removed after more testing, but no harm + + /* close uarts */ + close(ardrone_write); + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + ardrone_write = ardrone_open_uart(device, &uart_config_original); + + /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ + gpios = ar_multiplexing_init(); + + if (ardrone_write < 0) { + fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + thread_running = false; + exit(ERROR); + } + + /* initialize motors */ + if (OK != ar_init_motors(ardrone_write, gpios)) { + close(ardrone_write); + fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + thread_running = false; + exit(ERROR); + } + + while (!thread_should_exit) { + + if (motor_test_mode) { + /* set motors to idle speed */ + if (test_motor > 0 && test_motor < 5) { + int motors[4] = {0, 0, 0, 0}; + motors[test_motor - 1] = 10; + ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); + } else { + ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); + } + + } else { + /* MAIN OPERATION MODE */ + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of the actuator controls */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + /* for now only spin if armed and immediately shut down + * if in failsafe + */ + if (armed.armed && !armed.lockdown) { + ardrone_mixing_and_output(ardrone_write, &actuator_controls); + + } else { + /* Silently lock down motor speeds to zero */ + ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); + } + } + + if (counter % 24 == 0) { + if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + + if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + + if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + + if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + + if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + + if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + + if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + + if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + + if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + + if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + + led_counter++; + + if (led_counter == 12) led_counter = 0; + } + + /* run at approximately 200 Hz */ + usleep(4500); + + counter++; + } + + /* restore old UART config */ + int termios_state; + + if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { + fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); + } + + printf("[ardrone_interface] Restored original UART config, exiting..\n"); + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + fflush(stdout); + + thread_running = false; + + return OK; +} + diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c new file mode 100644 index 000000000..f15c74a22 --- /dev/null +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -0,0 +1,492 @@ +/**************************************************************************** + * + * 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 ardrone_motor_control.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ardrone_motor_control.h" + +static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; +static unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; + +typedef union { + uint16_t motor_value; + uint8_t bytes[2]; +} motor_union_t; + +#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */ + +/** + * @brief Generate the 8-byte motor set packet + * + * @return the number of bytes (8) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ + motor_buf[0] = 0x20; + motor_buf[1] = 0x00; + motor_buf[2] = 0x00; + motor_buf[3] = 0x00; + motor_buf[4] = 0x00; + /* + * {0x20, 0x00, 0x00, 0x00, 0x00}; + * 0x20 is start sign / motor command + */ + motor_union_t curr_motor; + uint16_t nineBitMask = 0x1FF; + + /* Set motor 1 */ + curr_motor.motor_value = (motor1 & nineBitMask) << 4; + motor_buf[0] |= curr_motor.bytes[1]; + motor_buf[1] |= curr_motor.bytes[0]; + + /* Set motor 2 */ + curr_motor.motor_value = (motor2 & nineBitMask) << 3; + motor_buf[1] |= curr_motor.bytes[1]; + motor_buf[2] |= curr_motor.bytes[0]; + + /* Set motor 3 */ + curr_motor.motor_value = (motor3 & nineBitMask) << 2; + motor_buf[2] |= curr_motor.bytes[1]; + motor_buf[3] |= curr_motor.bytes[0]; + + /* Set motor 4 */ + curr_motor.motor_value = (motor4 & nineBitMask) << 1; + motor_buf[3] |= curr_motor.bytes[1]; + motor_buf[4] |= curr_motor.bytes[0]; +} + +void ar_enable_broadcast(int fd) +{ + ar_select_motor(fd, 0); +} + +int ar_multiplexing_init() +{ + int fd; + + fd = open(GPIO_DEVICE_PATH, 0); + + if (fd < 0) { + warn("GPIO: open fail"); + return fd; + } + + /* deactivate all outputs */ + if (ioctl(fd, GPIO_SET, motor_gpios)) { + warn("GPIO: clearing pins fail"); + close(fd); + return -1; + } + + /* configure all motor select GPIOs as outputs */ + if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { + warn("GPIO: output set fail"); + close(fd); + return -1; + } + + return fd; +} + +int ar_multiplexing_deinit(int fd) +{ + if (fd < 0) { + printf("GPIO: no valid descriptor\n"); + return fd; + } + + int ret = 0; + + /* deselect motor 1-4 */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ret != 0) { + printf("GPIO: clear failed %d times\n", ret); + } + + if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) { + printf("GPIO: input set fail\n"); + return -1; + } + + close(fd); + + return ret; +} + +int ar_select_motor(int fd, uint8_t motor) +{ + int ret = 0; + /* + * Four GPIOS: + * GPIO_EXT1 + * GPIO_EXT2 + * GPIO_UART2_CTS + * GPIO_UART2_RTS + */ + + /* select motor 0 to enable broadcast */ + if (motor == 0) { + /* select motor 1-4 */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpios); + + } else { + /* select reqested motor */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); + } + + return ret; +} + +int ar_deselect_motor(int fd, uint8_t motor) +{ + int ret = 0; + /* + * Four GPIOS: + * GPIO_EXT1 + * GPIO_EXT2 + * GPIO_UART2_CTS + * GPIO_UART2_RTS + */ + + if (motor == 0) { + /* deselect motor 1-4 */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + } else { + /* deselect reqested motor */ + ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); + } + + return ret; +} + +int ar_init_motors(int ardrone_uart, int gpios) +{ + /* Write ARDrone commands on UART2 */ + uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; + uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; + + /* deselect all motors */ + ar_deselect_motor(gpios, 0); + + /* initialize all motors + * - select one motor at a time + * - configure motor + */ + int i; + int errcounter = 0; + + + /* initial setup run */ + for (i = 1; i < 5; ++i) { + /* Initialize motors 1-4 */ + errcounter += ar_select_motor(gpios, i); + usleep(200); + + /* + * write 0xE0 - request status + * receive one status byte + */ + write(ardrone_uart, &(initbuf[0]), 1); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*1); + + /* + * write 0x91 - request checksum + * receive 120 status bytes + */ + write(ardrone_uart, &(initbuf[1]), 1); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*120); + + /* + * write 0xA1 - set status OK + * receive one status byte - should be A0 + * to confirm status is OK + */ + write(ardrone_uart, &(initbuf[2]), 1); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*1); + + /* + * set as motor i, where i = 1..4 + * receive nothing + */ + initbuf[3] = (uint8_t)i; + write(ardrone_uart, &(initbuf[3]), 1); + fsync(ardrone_uart); + + /* + * write 0x40 - check version + * receive 11 bytes encoding the version + */ + write(ardrone_uart, &(initbuf[4]), 1); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US*11); + + ar_deselect_motor(gpios, i); + /* sleep 200 ms */ + usleep(200000); + } + + /* start the multicast part */ + errcounter += ar_select_motor(gpios, 0); + usleep(200); + + /* + * first round + * write six times A0 - enable broadcast + * receive nothing + */ + write(ardrone_uart, multicastbuf, sizeof(multicastbuf)); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf)); + + /* + * second round + * write six times A0 - enable broadcast + * receive nothing + */ + write(ardrone_uart, multicastbuf, sizeof(multicastbuf)); + fsync(ardrone_uart); + usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf)); + + /* set motors to zero speed (fsync is part of the write command */ + ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); + + if (errcounter != 0) { + fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); + fflush(stdout); + } + return errcounter; +} + +/** + * Sets the leds on the motor controllers, 1 turns led on, 0 off. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +{ + /* + * 2 bytes are sent. The first 3 bits describe the command: 011 means led control + * the following 4 bits are the red leds for motor 4, 3, 2, 1 + * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1 + * the last bit is unknown. + * + * The packet is therefore: + * 011 rrrr 0000 gggg 0 + */ + uint8_t leds[2]; + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + write(ardrone_uart, leds, 2); +} + +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { + const unsigned int min_motor_interval = 4900; + static uint64_t last_motor_time = 0; + + static struct actuator_outputs_s outputs; + outputs.timestamp = hrt_absolute_time(); + outputs.output[0] = motor1; + outputs.output[1] = motor2; + outputs.output[2] = motor3; + outputs.output[3] = motor4; + static orb_advert_t pub = 0; + if (pub == 0) { + pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + } + + if (hrt_absolute_time() - last_motor_time > min_motor_interval) { + uint8_t buf[5] = {0}; + ar_get_motor_packet(buf, motor1, motor2, motor3, motor4); + int ret; + ret = write(ardrone_fd, buf, sizeof(buf)); + fsync(ardrone_fd); + + /* publish just written values */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs); + + if (ret == sizeof(buf)) { + return OK; + } else { + return ret; + } + } else { + return -ERROR; + } +} + +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) { + + float roll_control = actuators->control[0]; + float pitch_control = actuators->control[1]; + float yaw_control = actuators->control[2]; + float motor_thrust = actuators->control[3]; + + //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); + + const float min_thrust = 0.02f; /**< 2% minimum thrust */ + const float max_thrust = 1.0f; /**< 100% max thrust */ + const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ + const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ + const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ + + /* initialize all fields to zero */ + uint16_t motor_pwm[4] = {0}; + float motor_calc[4] = {0}; + + float output_band = 0.0f; + float band_factor = 0.75f; + const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ + float yaw_factor = 1.0f; + + static bool initialized = false; + /* publish effective outputs */ + static struct actuator_controls_effective_s actuator_controls_effective; + static orb_advert_t actuator_controls_effective_pub; + + if (motor_thrust <= min_thrust) { + motor_thrust = min_thrust; + output_band = 0.0f; + } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { + output_band = band_factor * (motor_thrust - min_thrust); + } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * startpoint_full_control; + } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * (max_thrust - motor_thrust); + } + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer + + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); + + // if we are not in the output band + if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band + && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band + && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band + && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + + yaw_factor = 0.5f; + yaw_control *= yaw_factor; + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); + } + + for (int i = 0; i < 4; i++) { + //check for limits + if (motor_calc[i] < motor_thrust - output_band) { + motor_calc[i] = motor_thrust - output_band; + } + + if (motor_calc[i] > motor_thrust + output_band) { + motor_calc[i] = motor_thrust + output_band; + } + } + + /* publish effective outputs */ + actuator_controls_effective.control_effective[0] = roll_control; + actuator_controls_effective.control_effective[1] = pitch_control; + /* yaw output after limiting */ + actuator_controls_effective.control_effective[2] = yaw_control; + /* possible motor thrust limiting */ + actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; + + if (!initialized) { + /* advertise and publish */ + actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); + initialized = true; + } else { + /* already initialized, just publishing */ + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); + } + + /* set the motor values */ + + /* scale up from 0..1 to 10..512) */ + motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + + /* Keep motors spinning while armed and prevent overflows */ + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; + motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; + motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; + motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; + + /* send motors via UART */ + ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); +} diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h new file mode 100644 index 000000000..78b603b63 --- /dev/null +++ b/src/drivers/ardrone_interface/ardrone_motor_control.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 ardrone_motor_control.h + * Definition of AR.Drone 1.0 / 2.0 motor control interface + */ + +#include +#include + +/** + * Generate the 5-byte motor set packet. + * + * @return the number of bytes (5) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +/** + * Select a motor in the multiplexing. + * + * @param fd GPIO file descriptor + * @param motor Motor number, from 1 to 4, 0 selects all + */ +int ar_select_motor(int fd, uint8_t motor); + +/** + * Deselect a motor in the multiplexing. + * + * @param fd GPIO file descriptor + * @param motor Motor number, from 1 to 4, 0 deselects all + */ +int ar_deselect_motor(int fd, uint8_t motor); + +void ar_enable_broadcast(int fd); + +int ar_multiplexing_init(void); +int ar_multiplexing_deinit(int fd); + +/** + * Write four motor commands to an already initialized port. + * + * Writing 0 stops a motor, values from 1-512 encode the full thrust range. + * on some motor controller firmware revisions a minimum value of 10 is + * required to spin the motors. + */ +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +/** + * Initialize the motors. + */ +int ar_init_motors(int ardrone_uart, int gpio); + +/** + * Set LED pattern. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); + +/** + * Mix motors and output actuators + */ +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators); diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk new file mode 100644 index 000000000..058bd1397 --- /dev/null +++ b/src/drivers/ardrone_interface/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# AR.Drone motor driver +# + +MODULE_COMMAND = ardrone_interface +SRCS = ardrone_interface.c \ + ardrone_motor_control.c -- cgit v1.2.3 From 9169ceb3f4884a863d527c6b8e7ea237b41a48ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 11:10:39 +0200 Subject: Cut over commander app to new build system --- apps/commander/.context | 0 apps/commander/Makefile | 45 - apps/commander/calibration_routines.c | 219 --- apps/commander/calibration_routines.h | 61 - apps/commander/commander.c | 2181 -------------------------- apps/commander/commander.h | 58 - apps/commander/state_machine_helper.c | 752 --------- apps/commander/state_machine_helper.h | 209 --- makefiles/config_px4fmu_default.mk | 6 +- makefiles/config_px4fmuv2_default.mk | 5 + src/modules/commander/calibration_routines.c | 219 +++ src/modules/commander/calibration_routines.h | 61 + src/modules/commander/commander.c | 2181 ++++++++++++++++++++++++++ src/modules/commander/commander.h | 58 + src/modules/commander/module.mk | 41 + src/modules/commander/state_machine_helper.c | 752 +++++++++ src/modules/commander/state_machine_helper.h | 209 +++ 17 files changed, 3531 insertions(+), 3526 deletions(-) delete mode 100644 apps/commander/.context delete mode 100644 apps/commander/Makefile delete mode 100644 apps/commander/calibration_routines.c delete mode 100644 apps/commander/calibration_routines.h delete mode 100644 apps/commander/commander.c delete mode 100644 apps/commander/commander.h delete mode 100644 apps/commander/state_machine_helper.c delete mode 100644 apps/commander/state_machine_helper.h create mode 100644 src/modules/commander/calibration_routines.c create mode 100644 src/modules/commander/calibration_routines.h create mode 100644 src/modules/commander/commander.c create mode 100644 src/modules/commander/commander.h create mode 100644 src/modules/commander/module.mk create mode 100644 src/modules/commander/state_machine_helper.c create mode 100644 src/modules/commander/state_machine_helper.h diff --git a/apps/commander/.context b/apps/commander/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/commander/Makefile b/apps/commander/Makefile deleted file mode 100644 index d12697274..000000000 --- a/apps/commander/Makefile +++ /dev/null @@ -1,45 +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. -# -############################################################################ - -# -# Commander application -# - -APPNAME = commander -PRIORITY = SCHED_PRIORITY_MAX - 30 -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk - diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c deleted file mode 100644 index a26938637..000000000 --- a/apps/commander/calibration_routines.c +++ /dev/null @@ -1,219 +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 calibration_routines.c - * Calibration routines implementations. - * - * @author Lorenz Meier - */ - -#include - -#include "calibration_routines.h" - - -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) -{ - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A * A; - float B2 = B * B; - float C2 = C * C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * (QS - Rsq + QB + F0); - float aA, aB, aC, nA, nB, nC, dA, dB, dC; - - //Iterate N times, ignore stop condition. - int n = 0; - - while (n < max_iterations) { - n++; - - //Compute denominator: - aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); - aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); - aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; - - //Compute next iteration - nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); - nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); - nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - - if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A * A; - B2 = B * B; - C2 = C * C; - QS = A2 + B2 + C2; - QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * (QS - Rsq + QB + F0); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; -} diff --git a/apps/commander/calibration_routines.h b/apps/commander/calibration_routines.h deleted file mode 100644 index e3e7fbafd..000000000 --- a/apps/commander/calibration_routines.h +++ /dev/null @@ -1,61 +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 calibration_routines.h - * Calibration routines definitions. - * - * @author Lorenz Meier - */ - -/** - * Least-squares fit of a sphere to a set of points. - * - * Fits a sphere to a set of points on the sphere surface. - * - * @param x point coordinates on the X axis - * @param y point coordinates on the Y axis - * @param z point coordinates on the Z axis - * @param size number of points - * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. - * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. - * @param sphere_x coordinate of the sphere center on the X axis - * @param sphere_y coordinate of the sphere center on the Y axis - * @param sphere_z coordinate of the sphere center on the Z axis - * @param sphere_radius sphere radius - * - * @return 0 on success, 1 on failure - */ -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file diff --git a/apps/commander/commander.c b/apps/commander/commander.c deleted file mode 100644 index 7c0a25f86..000000000 --- a/apps/commander/commander.c +++ /dev/null @@ -1,2181 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * 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 commander.c - * Main system state machine implementation. - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - */ - -#include "commander.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ -#include -#include -#include -#include - -#include "calibration_routines.h" - - -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - -#include -extern struct system_load_s system_load; - -/* Decouple update interval and hysteris counters, all depends on intervals */ -#define COMMANDER_MONITORING_INTERVAL 50000 -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -/* File descriptors */ -static int leds; -static int buzzer; -static int mavlink_fd; -static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; - -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -static unsigned int failsafe_lowlevel_timeout_ms; - -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -/* pthread loops */ -static void *orb_receive_loop(void *arg); - -__EXPORT int commander_main(int argc, char *argv[]); - -/** - * Mainloop of commander. - */ -int commander_thread_main(int argc, char *argv[]); - -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); - -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - -static int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -static void buzzer_deinit() -{ - close(buzzer); -} - - -static int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -static void led_deinit() -{ - close(leds); -} - -static int led_toggle(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_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -static int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; - -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) -{ - - /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } - - /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } - - /* Trigger Tetris on being bored */ - - return 0; -} - -void tune_confirm(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_error(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) -{ - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) -{ - - /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - tune_confirm(); - sleep(2); - tune_confirm(); - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_confirm(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(sub_mag); -} - -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - -void do_accel_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it level and still"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 2500; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float accel_offset[3] = {0.0f, 0.0f, 0.0f}; - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - accel_offset[0] += raw.accelerometer_m_s2[0]; - accel_offset[1] += raw.accelerometer_m_s2[1]; - accel_offset[2] += raw.accelerometer_m_s2[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); - return; - } - } - - accel_offset[0] = accel_offset[0] / calibration_count; - accel_offset[1] = accel_offset[1] / calibration_count; - accel_offset[2] = accel_offset[2] / calibration_count; - - if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { - - /* add the removed length from x / y to z, since we induce a scaling issue else */ - float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]); - - /* if length is correct, zero results here */ - accel_offset[2] = accel_offset[2] + total_len; - - float scale = 9.80665f / total_len; - - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(scale)) - || param_set(param_find("SENS_ACC_YSCALE"), &(scale)) - || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); - } - - fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offset[0], - scale, - accel_offset[1], - scale, - accel_offset[2], - scale, - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "accel calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); - } - - /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(sub_sensor_combined); -} - -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it still"); - /* set to accel calibration mode */ - status->flag_preflight_airspeed_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 2500; - - int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s differential_pressure; - - int calibration_counter = 0; - float airspeed_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); - airspeed_offset += differential_pressure.voltage; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - airspeed_offset = airspeed_offset / calibration_count; - - if (isfinite(airspeed_offset)) { - - if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - /* exit airspeed calibration mode */ - status->flag_preflight_airspeed_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(sub_differential_pressure); -} - - - -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* announce command handling */ - tune_confirm(); - - - /* supported command handling start */ - - /* request to set different system mode */ - switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - /* request to disarm */ - - } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - } - } - } - break; - -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); - - } else { - - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd->param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - } - break; - - default: { - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); - } - break; - } - - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); - - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0) { - /* send acknowledge command */ - // XXX TODO - } - -} - -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int commander_thread_main(int argc, char *argv[]) -{ - /* not yet initialized */ - commander_initialized = false; - bool home_position_set = false; - - /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - - /* welcome user */ - warnx("I am in command now!\n"); - - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; - - /* initialize */ - if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); - } - - if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); - } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); - } - - /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; - current_status.flag_system_armed = false; - /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; - /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; - /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - - /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); - /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); - - /* home position */ - orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); - - if (stat_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } - - mavlink_log_info(mavlink_fd, "system is running"); - - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - - /* Start monitoring loop */ - uint16_t counter = 0; - uint8_t flight_env; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; - - /* Subscribe to manual control data */ - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; - memset(&sp_man, 0, sizeof(sp_man)); - - /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); - - int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_s global_position; - memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; - - int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; - - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - - int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s differential_pressure; - memset(&differential_pressure, 0, sizeof(differential_pressure)); - uint64_t last_differential_pressure_time = 0; - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - - /* Subscribe to parameters changed topic */ - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); - - /* subscribe to battery topic */ - int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; - - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - - /* now initialized */ - commander_initialized = true; - thread_running = true; - - uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; - - while (!thread_should_exit) { - - /* Get current values */ - bool new_data; - orb_check(sp_man_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } - - orb_check(sp_offboard_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); - } - - orb_check(sensor_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - } - - orb_check(differential_pressure_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); - last_differential_pressure_time = differential_pressure.timestamp; - } - - orb_check(cmd_sub, &new_data); - - if (new_data) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); - } - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } - } - - /* update global position estimate */ - orb_check(global_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; - } - - /* update local position estimate */ - orb_check(local_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; - } - - /* update battery status */ - orb_check(battery_sub, &new_data); - if (new_data) { - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); - - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_BLUE); - } - - /* toggle error led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_AMBER); - - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); - - // } else { - // led_off(LED_AMBER); - // } - } - - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } - } - - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { - /* For less than 20%, start be slightly annoying at 1 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - } - - low_voltage_counter++; - } - - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); - } - - critical_voltage_counter++; - - } else { - low_voltage_counter = 0; - critical_voltage_counter = 0; - } - - /* End battery voltage check */ - - - /* - * Check for valid position information. - * - * If the system has a valid position source from an onboard - * position estimator, it is safe to operate it autonomously. - * The flag_vector_flight_mode_ok flag indicates that a minimum - * set of position measurements is available. - */ - - /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; - bool airspeed_valid = current_status.flag_airspeed_valid; - - /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_global_position_valid = false; - } - - if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { - current_status.flag_airspeed_valid = true; - - } else { - current_status.flag_airspeed_valid = false; - } - - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; - - } else { - current_status.flag_vector_flight_mode_ok = false; - } - - /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid || - airspeed_valid != current_status.flag_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ - - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } - - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { - - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - - /* check if gps fix is ok */ - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { - warnx("setting home position"); - - /* copy position data to uORB home message, store it locally as well */ - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - home_position_set = true; - tune_confirm(); - } - } - - /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if manual stability control modes have to be switched - */ - if (!isfinite(sp_man.manual_sas_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; - - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; - - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; - - } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; - } - - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; - - } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; - - } else { - stick_on_counter++; - stick_off_counter = 0; - } - } - - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; - - } else { - static uint64_t last_print_time = 0; - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; - state_changed = true; - } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } - } - } - - - - - /* End mode switch */ - - /* END RC state check */ - - - /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; - - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } - - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } - - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_confirm(); - - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_confirm(); - } - } - - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; - - /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - - } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - } - - } else { - static uint64_t last_print_time = 0; - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); - - /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; - state_changed = true; - } - } - } - } - - - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - - - /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); - state_changed = false; - } - - /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; - - fflush(stdout); - counter++; - usleep(COMMANDER_MONITORING_INTERVAL); - } - - /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); - - /* close fds */ - led_deinit(); - buzzer_deinit(); - close(sp_man_sub); - close(sp_offboard_sub); - close(global_position_sub); - close(sensor_sub); - close(cmd_sub); - - warnx("exiting..\n"); - fflush(stdout); - - thread_running = false; - - return 0; -} diff --git a/apps/commander/commander.h b/apps/commander/commander.h deleted file mode 100644 index 04b4e72ab..000000000 --- a/apps/commander/commander.h +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * 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 commander.h - * Main system state machine definition. - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - */ - -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - -void tune_confirm(void); -void tune_error(void); - -#endif /* COMMANDER_H_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c deleted file mode 100644 index bea388a10..000000000 --- a/apps/commander/state_machine_helper.c +++ /dev/null @@ -1,752 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * 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 state_machine_helper.c - * State machine helper functions implementations - */ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" - -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; - int ret = ERROR; - - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); - - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } - } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; - - case SYSTEM_STATE_GROUND_ERROR: - - /* set system flags according to state */ - - /* prevent actuators from arming */ - current_status->flag_system_armed = false; - - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; - - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } - - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: - - /* set system flags according to state */ - - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - - case SYSTEM_STATE_AUTO: - - /* set system flags according to state */ - - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; - - case SYSTEM_STATE_STABILIZED: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; - - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; - } - - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; - } - - return ret; -} - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} - -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} - - -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} - - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h deleted file mode 100644 index 2f2ccc729..000000000 --- a/apps/commander/state_machine_helper.h +++ /dev/null @@ -1,209 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * 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 state_machine_helper.h - * State machine helper functions definitions - */ - -#ifndef STATE_MACHINE_HELPER_H_ -#define STATE_MACHINE_HELPER_H_ - -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - -#include -#include - -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); - -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - - -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - - -#endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 1302d1582..6c43377e3 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -17,6 +17,11 @@ MODULES += drivers/l3gd20 MODULES += drivers/ardrone_interface MODULES += systemcmds/eeprom +# +# General system control +# +MODULES += modules/commander + # # Estimation modules (EKF / other filters) # @@ -41,7 +46,6 @@ BUILTIN_COMMANDS := \ $(call _B, blinkm, , 2048, blinkm_main ) \ $(call _B, bma180, , 2048, bma180_main ) \ $(call _B, boardinfo, , 2048, boardinfo_main ) \ - $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_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 ) \ diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 659b9c95b..fd69baa29 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -17,6 +17,11 @@ MODULES += drivers/px4fmu MODULES += drivers/rgbled MODULES += systemcmds/ramtron +# +# General system control +# +MODULES += modules/commander + # # Estimation modules (EKF / other filters) # diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c new file mode 100644 index 000000000..a26938637 --- /dev/null +++ b/src/modules/commander/calibration_routines.c @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 calibration_routines.c + * Calibration routines implementations. + * + * @author Lorenz Meier + */ + +#include + +#include "calibration_routines.h" + + +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) +{ + + float x_sumplain = 0.0f; + float x_sumsq = 0.0f; + float x_sumcube = 0.0f; + + float y_sumplain = 0.0f; + float y_sumsq = 0.0f; + float y_sumcube = 0.0f; + + float z_sumplain = 0.0f; + float z_sumsq = 0.0f; + float z_sumcube = 0.0f; + + float xy_sum = 0.0f; + float xz_sum = 0.0f; + float yz_sum = 0.0f; + + float x2y_sum = 0.0f; + float x2z_sum = 0.0f; + float y2x_sum = 0.0f; + float y2z_sum = 0.0f; + float z2x_sum = 0.0f; + float z2y_sum = 0.0f; + + for (unsigned int i = 0; i < size; i++) { + + float x2 = x[i] * x[i]; + float y2 = y[i] * y[i]; + float z2 = z[i] * z[i]; + + x_sumplain += x[i]; + x_sumsq += x2; + x_sumcube += x2 * x[i]; + + y_sumplain += y[i]; + y_sumsq += y2; + y_sumcube += y2 * y[i]; + + z_sumplain += z[i]; + z_sumsq += z2; + z_sumcube += z2 * z[i]; + + xy_sum += x[i] * y[i]; + xz_sum += x[i] * z[i]; + yz_sum += y[i] * z[i]; + + x2y_sum += x2 * y[i]; + x2z_sum += x2 * z[i]; + + y2x_sum += y2 * x[i]; + y2z_sum += y2 * z[i]; + + z2x_sum += z2 * x[i]; + z2y_sum += z2 * y[i]; + } + + // + //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data + // + // P is a structure that has been computed with the data earlier. + // P.npoints is the number of elements; the length of X,Y,Z are identical. + // P's members are logically named. + // + // X[n] is the x component of point n + // Y[n] is the y component of point n + // Z[n] is the z component of point n + // + // A is the x coordiante of the sphere + // B is the y coordiante of the sphere + // C is the z coordiante of the sphere + // Rsq is the radius squared of the sphere. + // + //This method should converge; maybe 5-100 iterations or more. + // + float x_sum = x_sumplain / size; //sum( X[n] ) + float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) + float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) + float y_sum = y_sumplain / size; //sum( Y[n] ) + float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) + float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) + float z_sum = z_sumplain / size; //sum( Z[n] ) + float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) + float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) + + float XY = xy_sum / size; //sum( X[n] * Y[n] ) + float XZ = xz_sum / size; //sum( X[n] * Z[n] ) + float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) + float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) + float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) + float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) + float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) + float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) + float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) + + //Reduction of multiplications + float F0 = x_sum2 + y_sum2 + z_sum2; + float F1 = 0.5f * F0; + float F2 = -8.0f * (x_sum3 + Y2X + Z2X); + float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); + float F4 = -8.0f * (X2Z + Y2Z + z_sum3); + + //Set initial conditions: + float A = x_sum; + float B = y_sum; + float C = z_sum; + + //First iteration computation: + float A2 = A * A; + float B2 = B * B; + float C2 = C * C; + float QS = A2 + B2 + C2; + float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + + //Set initial conditions: + float Rsq = F0 + QB + QS; + + //First iteration computation: + float Q0 = 0.5f * (QS - Rsq); + float Q1 = F1 + Q0; + float Q2 = 8.0f * (QS - Rsq + QB + F0); + float aA, aB, aC, nA, nB, nC, dA, dB, dC; + + //Iterate N times, ignore stop condition. + int n = 0; + + while (n < max_iterations) { + n++; + + //Compute denominator: + aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); + aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); + aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); + aA = (aA == 0.0f) ? 1.0f : aA; + aB = (aB == 0.0f) ? 1.0f : aB; + aC = (aC == 0.0f) ? 1.0f : aC; + + //Compute next iteration + nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); + nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); + nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); + + //Check for stop condition + dA = (nA - A); + dB = (nB - B); + dC = (nC - C); + + if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } + + //Compute next iteration's values + A = nA; + B = nB; + C = nC; + A2 = A * A; + B2 = B * B; + C2 = C * C; + QS = A2 + B2 + C2; + QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + Rsq = F0 + QB + QS; + Q0 = 0.5f * (QS - Rsq); + Q1 = F1 + Q0; + Q2 = 8.0f * (QS - Rsq + QB + F0); + } + + *sphere_x = A; + *sphere_y = B; + *sphere_z = C; + *sphere_radius = sqrtf(Rsq); + + return 0; +} diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h new file mode 100644 index 000000000..e3e7fbafd --- /dev/null +++ b/src/modules/commander/calibration_routines.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 calibration_routines.h + * Calibration routines definitions. + * + * @author Lorenz Meier + */ + +/** + * Least-squares fit of a sphere to a set of points. + * + * Fits a sphere to a set of points on the sphere surface. + * + * @param x point coordinates on the X axis + * @param y point coordinates on the Y axis + * @param z point coordinates on the Z axis + * @param size number of points + * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. + * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. + * @param sphere_x coordinate of the sphere center on the X axis + * @param sphere_y coordinate of the sphere center on the Y axis + * @param sphere_z coordinate of the sphere center on the Z axis + * @param sphere_radius sphere radius + * + * @return 0 on success, 1 on failure + */ +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius); \ No newline at end of file diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c new file mode 100644 index 000000000..7c0a25f86 --- /dev/null +++ b/src/modules/commander/commander.c @@ -0,0 +1,2181 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * 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 commander.c + * Main system state machine implementation. + * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + */ + +#include "commander.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "state_machine_helper.h" +#include "systemlib/systemlib.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ +#include +#include +#include +#include + +#include "calibration_routines.h" + + +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +#include +extern struct system_load_s system_load; + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +/* File descriptors */ +static int leds; +static int buzzer; +static int mavlink_fd; +static bool commander_initialized = false; +static struct vehicle_status_s current_status; /**< Main state machine */ +static orb_advert_t stat_pub; + +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; + +static unsigned int failsafe_lowlevel_timeout_ms; + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* pthread loops */ +static void *orb_receive_loop(void *arg); + +__EXPORT int commander_main(int argc, char *argv[]); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +static int buzzer_init(void); +static void buzzer_deinit(void); +static int led_init(void); +static void led_deinit(void); +static int led_toggle(int led); +static int led_on(int led); +static int led_off(int led); +static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); +static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); +static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); +static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); + + + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +/** + * Sort calibration values. + * + * Sorts the calibration values with bubble sort. + * + * @param a The array to sort + * @param n The number of entries in the array + */ +// static void cal_bsort(float a[], int n); + +static int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +static void buzzer_deinit() +{ + close(buzzer); +} + + +static int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +static void led_deinit() +{ + close(leds); +} + +static int led_toggle(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_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +static int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +enum AUDIO_PATTERN { + AUDIO_PATTERN_ERROR = 2, + AUDIO_PATTERN_NOTIFY_POSITIVE = 3, + AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, + AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, + AUDIO_PATTERN_NOTIFY_CHARGE = 6 +}; + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +{ + + /* Trigger alarm if going into any error state */ + if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || + ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); + } + + /* Trigger neutral on arming / disarming */ + if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); + } + + /* Trigger Tetris on being bored */ + + return 0; +} + +void tune_confirm(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_error(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +{ + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + return; + } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} + +void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +{ + + /* set to mag calibration mode */ + status->flag_preflight_mag_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + // XXX old cal + // * FLT_MIN is not the most negative float number, + // * but the smallest number by magnitude float can + // * represent. Use -FLT_MAX to initialize the most + // * negative number + + // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + tune_confirm(); + sleep(2); + tune_confirm(); + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_confirm(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + /* disable calibration mode */ + status->flag_preflight_mag_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_mag); +} + +void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* set to gyro calibration mode */ + status->flag_preflight_gyro_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + /* exit gyro calibration mode */ + status->flag_preflight_gyro_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it level and still"); + /* set to accel calibration mode */ + status->flag_preflight_accel_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float accel_offset[3] = {0.0f, 0.0f, 0.0f}; + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + accel_offset[0] += raw.accelerometer_m_s2[0]; + accel_offset[1] += raw.accelerometer_m_s2[1]; + accel_offset[2] += raw.accelerometer_m_s2[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); + return; + } + } + + accel_offset[0] = accel_offset[0] / calibration_count; + accel_offset[1] = accel_offset[1] / calibration_count; + accel_offset[2] = accel_offset[2] / calibration_count; + + if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { + + /* add the removed length from x / y to z, since we induce a scaling issue else */ + float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]); + + /* if length is correct, zero results here */ + accel_offset[2] = accel_offset[2] + total_len; + + float scale = 9.80665f / total_len; + + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { + mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); + } + + fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offset[0], + scale, + accel_offset[1], + scale, + accel_offset[2], + scale, + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "accel calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); + } + + /* exit accel calibration mode */ + status->flag_preflight_accel_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_sensor_combined); +} + +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + /* set to accel calibration mode */ + status->flag_preflight_airspeed_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + + int calibration_counter = 0; + float airspeed_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); + airspeed_offset += differential_pressure.voltage; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + airspeed_offset = airspeed_offset / calibration_count; + + if (isfinite(airspeed_offset)) { + + if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + /* exit airspeed calibration mode */ + status->flag_preflight_airspeed_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_differential_pressure); +} + + + +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* announce command handling */ + tune_confirm(); + + + /* supported command handling start */ + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + /* request to disarm */ + + } else if ((int)cmd->param1 == 0) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + + /* request for an autopilot reboot */ + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + if ((int)cmd->param1 == 1) { + if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + +// /* request to land */ +// case VEHICLE_CMD_NAV_LAND: +// { +// //TODO: add check if landing possible +// //TODO: add landing maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// } } +// break; +// +// /* request to takeoff */ +// case VEHICLE_CMD_NAV_TAKEOFF: +// { +// //TODO: add check if takeoff possible +// //TODO: add takeoff maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// } +// } +// break; +// + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); + tune_confirm(); + do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished gyro cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting mag cal"); + tune_confirm(); + do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + tune_confirm(); + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting trim cal"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished trim cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting accel cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished accel cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + } + break; + + default: { + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* announce command rejection */ + ioctl(buzzer, TONE_SET_ALARM, 4); + } + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + ioctl(buzzer, TONE_SET_ALARM, 5); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_confirm(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander orb rcv", getpid()); + + /* Subscribe to command topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + + struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; + + while (!thread_should_exit) { + struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; + + if (poll(fds, 1, 5000) == 0) { + /* timeout, but this is no problem, silently ignore */ + } else { + /* got command */ + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + vstatus->onboard_control_sensors_present |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + vstatus->onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + vstatus->onboard_control_sensors_health |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_health &= ~info.subsystem_type; + } + } + } + + close(subsys_sub); + + return NULL; +} + +/* + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + + /* welcome user */ + warnx("I am in command now!\n"); + + /* pthreads for command and subsystem info handling */ + // pthread_t command_handling_thread; + pthread_t subsystem_info_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds\n"); + } + + if (buzzer_init() != 0) { + warnx("ERROR: Failed to initialize buzzer\n"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + } + + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ + current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ + current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + + /* advertise to ORB */ + stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ + state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (stat_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + mavlink_log_info(mavlink_fd, "system is running"); + + /* create pthreads */ + pthread_attr_t subsystem_info_attr; + pthread_attr_init(&subsystem_info_attr); + pthread_attr_setstacksize(&subsystem_info_attr, 2048); + pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + + /* Start monitoring loop */ + uint16_t counter = 0; + uint8_t flight_env; + + /* Initialize to 0.0V */ + float battery_voltage = 0.0f; + bool battery_voltage_valid = false; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; + uint8_t low_voltage_counter = 0; + uint16_t critical_voltage_counter = 0; + int16_t mode_switch_rc_value; + float bat_remain = 1.0f; + + uint16_t stick_off_counter = 0; + uint16_t stick_on_counter = 0; + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + memset(&differential_pressure, 0, sizeof(differential_pressure)); + uint64_t last_differential_pressure_time = 0; + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + // uint8_t vehicle_state_previous = current_status.state_machine; + float voltage_previous = 0.0f; + + uint64_t last_idle_time = 0; + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + uint64_t start_time = hrt_absolute_time(); + uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + while (!thread_should_exit) { + + /* Get current values */ + bool new_data; + orb_check(sp_man_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(differential_pressure_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); + last_differential_pressure_time = differential_pressure.timestamp; + } + + orb_check(cmd_sub, &new_data); + + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(stat_pub, ¤t_status, &cmd); + } + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + battery_voltage = battery.voltage_v; + battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (hrt_absolute_time() - start_time > 2500000) { + bat_remain = battery_remaining_estimate_voltage(battery_voltage); + } + } + + /* Slow but important 8 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ + if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || + current_status.state_machine == SYSTEM_STATE_AUTO || + current_status.state_machine == SYSTEM_STATE_MANUAL)) { + /* armed */ + led_toggle(LED_BLUE); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } + + /* toggle error led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_AMBER); + + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); + + } else { + // /* Constant error indication in standby mode without GPS */ + // if (!current_status.gps_valid) { + // led_on(LED_AMBER); + + // } else { + // led_off(LED_AMBER); + // } + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + } + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { + /* For less than 10%, start be really annoying at 5 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, 3); + + } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { + /* For less than 20%, start be slightly annoying at 1 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + tune_confirm(); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + } + + /* Check battery voltage */ + /* write to sys_status */ + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + + } else { + current_status.voltage_battery = 0.0f; + } + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.flag_global_position_valid; + bool local_pos_valid = current_status.flag_local_position_valid; + bool airspeed_valid = current_status.flag_airspeed_valid; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + current_status.flag_airspeed_valid = true; + + } else { + current_status.flag_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + + } else { + current_status.flag_vector_flight_mode_ok = false; + } + + /* consolidate state change, flag as changed if required */ + if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + global_pos_valid != current_status.flag_global_position_valid || + local_pos_valid != current_status.flag_local_position_valid || + airspeed_valid != current_status.flag_airspeed_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } + + if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; + + /* check if gps fix is ok */ + // XXX magic number + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !current_status.flag_system_armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_confirm(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* Start RC state check */ + + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + + // /* + // * Check if manual control modes have to be switched + // */ + // if (!isfinite(sp_man.manual_mode_switch)) { + // warnx("man mode sw not finite\n"); + + // /* this switch is not properly mapped, set default */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, fall back to direct pass-through */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + // /* bottom stick position, set direct mode for vehicles supporting it */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, set to direct pass-through as requested */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + // /* top stick position, set SAS for all vehicle types */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + + // } else { + // /* center stick position, set rate control */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = true; + // } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && + (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { + // /* check auto mode switch for correct mode */ + // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + // /* enable guided mode */ + // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + + // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // XXX hardcode to auto for now + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + + // } + + } else { + /* center stick position, set SAS for all vehicle types */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + static uint64_t last_print_time = 0; + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + state_changed = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } + } + } + + + + + /* End mode switch */ + + /* END RC state check */ + + + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + /* decide about attitude control flag, enable in att/pos/vel */ + bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + /* decide about rate control flag, enable it always XXX (for now) */ + bool rates_ctrl_enabled = true; + + /* set up control mode */ + if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + state_changed = true; + } + + if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + current_status.flag_control_rates_enabled = rates_ctrl_enabled; + state_changed = true; + } + + /* handle the case where offboard control signal was regained */ + if (!current_status.offboard_control_signal_found_once) { + current_status.offboard_control_signal_found_once = true; + /* enable offboard control, disable manual input */ + current_status.flag_control_manual_enabled = false; + current_status.flag_control_offboard_enabled = true; + state_changed = true; + tune_confirm(); + + mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + } else { + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + state_changed = true; + tune_confirm(); + } + } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + /* arm / disarm on request */ + if (sp_offboard.armed && !current_status.flag_system_armed) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + /* switch to stabilized mode = takeoff */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + + } else if (!sp_offboard.armed && current_status.flag_system_armed) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + } + + } else { + static uint64_t last_print_time = 0; + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + tune_confirm(); + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + current_status.failsave_lowlevel = true; + state_changed = true; + } + } + } + } + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + + + /* If full run came back clean, transition to standby */ + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + current_status.flag_preflight_gyro_calibration == false && + current_status.flag_preflight_mag_calibration == false && + current_status.flag_preflight_accel_calibration == false) { + /* All ok, no calibration going on, go to standby */ + do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + state_changed = false; + } + + /* Store old modes to detect and act on state transitions */ + voltage_previous = current_status.voltage_battery; + + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + // pthread_join(command_handling_thread, NULL); + pthread_join(subsystem_info_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(global_position_sub); + close(sensor_sub); + close(cmd_sub); + + warnx("exiting..\n"); + fflush(stdout); + + thread_running = false; + + return 0; +} diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h new file mode 100644 index 000000000..04b4e72ab --- /dev/null +++ b/src/modules/commander/commander.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * 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 commander.h + * Main system state machine definition. + * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + */ + +#ifndef COMMANDER_H_ +#define COMMANDER_H_ + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +void tune_confirm(void); +void tune_error(void); + +#endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk new file mode 100644 index 000000000..b90da8289 --- /dev/null +++ b/src/modules/commander/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. +# +############################################################################ + +# +# Main system state machine +# + +MODULE_COMMAND = commander +SRCS = commander.c \ + state_machine_helper.c \ + calibration_routines.c diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c new file mode 100644 index 000000000..bea388a10 --- /dev/null +++ b/src/modules/commander/state_machine_helper.c @@ -0,0 +1,752 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 state_machine_helper.c + * State machine helper functions implementations + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "state_machine_helper.h" + +static const char *system_state_txt[] = { + "SYSTEM_STATE_PREFLIGHT", + "SYSTEM_STATE_STANDBY", + "SYSTEM_STATE_GROUND_READY", + "SYSTEM_STATE_MANUAL", + "SYSTEM_STATE_STABILIZED", + "SYSTEM_STATE_AUTO", + "SYSTEM_STATE_MISSION_ABORT", + "SYSTEM_STATE_EMCY_LANDING", + "SYSTEM_STATE_EMCY_CUTOFF", + "SYSTEM_STATE_GROUND_ERROR", + "SYSTEM_STATE_REBOOT", + +}; + +/** + * Transition from one state to another + */ +int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +{ + int invalid_state = false; + int ret = ERROR; + + commander_state_machine_t old_state = current_status->state_machine; + + switch (new_state) { + case SYSTEM_STATE_MISSION_ABORT: { + /* Indoor or outdoor */ + // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { + ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + + // } else { + // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); + // } + } + break; + + case SYSTEM_STATE_EMCY_LANDING: + /* Tell the controller to land */ + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + warnx("EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + /* Tell the controller to cutoff the motors (thrust = 0) */ + + /* set system flags according to state */ + current_status->flag_system_armed = false; + + warnx("EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); + break; + + case SYSTEM_STATE_GROUND_ERROR: + + /* set system flags according to state */ + + /* prevent actuators from arming */ + current_status->flag_system_armed = false; + + warnx("GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); + break; + + case SYSTEM_STATE_PREFLIGHT: + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); + } + + break; + + case SYSTEM_STATE_REBOOT: + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { + invalid_state = false; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + case SYSTEM_STATE_STANDBY: + /* set system flags according to state */ + + /* standby enforces disarmed */ + current_status->flag_system_armed = false; + + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + break; + + case SYSTEM_STATE_GROUND_READY: + + /* set system flags according to state */ + + /* ground ready has motors / actuators armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); + break; + + case SYSTEM_STATE_AUTO: + + /* set system flags according to state */ + + /* auto is airborne and in auto mode, motors armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); + break; + + case SYSTEM_STATE_STABILIZED: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); + break; + + case SYSTEM_STATE_MANUAL: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); + break; + + default: + invalid_state = true; + break; + } + + if (invalid_state == false || old_state != new_state) { + current_status->state_machine = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + ret = OK; + } + + if (invalid_state) { + mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); + ret = ERROR; + } + + return ret; +} + +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* publish the new state */ + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + /* assemble state vector based on flag values */ + if (current_status->flag_control_rates_enabled) { + current_status->onboard_control_sensors_present |= 0x400; + + } else { + current_status->onboard_control_sensors_present &= ~0x400; + } + + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); +} + +void publish_armed_status(const struct vehicle_status_s *current_status) +{ + struct actuator_armed_s armed; + armed.armed = current_status->flag_system_armed; + /* lock down actuators if required, only in HIL */ + armed.lockdown = (current_status->flag_hil_enabled) ? true : false; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +} + + +/* + * Private functions, update the state machine + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + warnx("EMERGENCY HANDLER\n"); + /* Depending on the current state go to one of the error states */ + + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); + + } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { + + // DO NOT abort mission + //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + + } else { + warnx("Unknown system state: #%d\n", current_status->state_machine); + } +} + +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +{ + if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself + state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); + + } else { + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + } + +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //TODO state machine change (recovering) +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_GPS: +// //TODO state machine change +// break; + +// default: +// break; +// } + + +// } + + +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); +// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_GPS: +// // //TODO: remove this block +// // break; +// // /////////////////// +// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); + +// // printf("previosly_healthy = %u\n", previosly_healthy); +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency(status_pub, current_status); + +// break; + +// default: +// break; +// } + +// } + + +/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ + + +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { + state_machine_emergency(status_pub, current_status, mavlink_fd); + } +} + +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + printf("[cmd] arming\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + } +} + +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + printf("[cmd] going standby\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + + } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] MISSION ABORT!\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + + current_status->flag_control_manual_enabled = true; + + /* set behaviour based on airframe */ + if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, set to SAS */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + + } else { + + /* assuming a fixed wing, set to direct pass-through */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = false; + } + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] manual mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + } +} + +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + int old_mode = current_status->flight_mode; + int old_manual_control_mode = current_status->manual_control_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + current_status->flag_control_manual_enabled = true; + + if (old_mode != current_status->flight_mode || + old_manual_control_mode != current_status->manual_control_mode) { + printf("[cmd] att stabilized mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + state_machine_publish(status_pub, current_status, mavlink_fd); + } + + } +} + +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); + tune_error(); + return; + } + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] position guided mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + } +} + +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + } +} + + +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +{ + uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") + + } else { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") + } + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } + + /* vehicle is disarmed, mode requests arming */ + if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only arm in standby state */ + // XXX REMOVE + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + ret = OK; + printf("[cmd] arming due to command request\n"); + } + } + + /* vehicle is armed, mode requests disarming */ + if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only disarm in ground ready */ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + ret = OK; + printf("[cmd] disarming due to command request\n"); + } + } + + /* NEVER actually switch off HIL without reboot */ + if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { + warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); + ret = ERROR; + } + + return ret; +} + +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations +{ + commander_state_machine_t current_system_state = current_status->state_machine; + + uint8_t ret = 1; + + switch (custom_mode) { + case SYSTEM_STATE_GROUND_READY: + break; + + case SYSTEM_STATE_STANDBY: + break; + + case SYSTEM_STATE_REBOOT: + printf("try to reboot\n"); + + if (current_system_state == SYSTEM_STATE_STANDBY + || current_system_state == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { + printf("system will reboot\n"); + mavlink_log_critical(mavlink_fd, "Rebooting.."); + usleep(200000); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); + ret = 0; + } + + break; + + case SYSTEM_STATE_AUTO: + printf("try to switch to auto/takeoff\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + printf("state: auto\n"); + ret = 0; + } + + break; + + case SYSTEM_STATE_MANUAL: + printf("try to switch to manual\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + printf("state: manual\n"); + ret = 0; + } + + break; + + default: + break; + } + + return ret; +} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h new file mode 100644 index 000000000..2f2ccc729 --- /dev/null +++ b/src/modules/commander/state_machine_helper.h @@ -0,0 +1,209 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 state_machine_helper.h + * State machine helper functions definitions + */ + +#ifndef STATE_MACHINE_HELPER_H_ +#define STATE_MACHINE_HELPER_H_ + +#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) +#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock + +#include +#include + +/** + * Switch to new state with no checking. + * + * do_state_update: this is the functions that all other functions have to call in order to update the state. + * the function does not question the state change, this must be done before + * The function performs actions that are connected with the new state (buzzer, reboot, ...) + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + * + * @return 0 (macro OK) or 1 on error (macro ERROR) + */ +int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); + +/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + + +/** + * Handle state machine if got position fix + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if position fix lost + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if user wants to arm + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if user wants to disarm + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is manual + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is stabilized + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is guided + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is auto + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Publish current state information + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + + +/* + * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). + * If the request is obeyed the functions return 0 + * + */ + +/** + * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); + +/** + * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); + +/** + * Always switches to critical mode under any circumstances. + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Switches to emergency if required. + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Publish the armed state depending on the current system state + * + * @param current_status the current system status + */ +void publish_armed_status(const struct vehicle_status_s *current_status); + + + +#endif /* STATE_MACHINE_HELPER_H_ */ -- cgit v1.2.3 From a39d5230be4ab841cbdf1210a6ca62901bce12b4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 15:03:46 -0700 Subject: Cull .context files. --- .gitignore | 1 + apps/mavlink/.context | 0 apps/position_estimator/.context | 0 apps/px4/tests/.context | 0 apps/sensors/.context | 0 apps/systemcmds/boardinfo/.context | 0 apps/systemcmds/perf/.context | 0 apps/systemcmds/reboot/.context | 0 apps/systemcmds/top/.context | 0 apps/uORB/.context | 0 10 files changed, 1 insertion(+) delete mode 100644 apps/mavlink/.context delete mode 100644 apps/position_estimator/.context delete mode 100644 apps/px4/tests/.context delete mode 100644 apps/sensors/.context delete mode 100644 apps/systemcmds/boardinfo/.context delete mode 100644 apps/systemcmds/perf/.context delete mode 100644 apps/systemcmds/reboot/.context delete mode 100644 apps/systemcmds/top/.context delete mode 100644 apps/uORB/.context diff --git a/.gitignore b/.gitignore index 91358e1e1..de03b0a60 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ .built +.context *.context *.bdat *.pdat diff --git a/apps/mavlink/.context b/apps/mavlink/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/position_estimator/.context b/apps/position_estimator/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/sensors/.context b/apps/sensors/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/boardinfo/.context b/apps/systemcmds/boardinfo/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/perf/.context b/apps/systemcmds/perf/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/reboot/.context b/apps/systemcmds/reboot/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/systemcmds/top/.context b/apps/systemcmds/top/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/uORB/.context b/apps/uORB/.context deleted file mode 100644 index e69de29bb..000000000 -- cgit v1.2.3 From 68c8de4cf13dd4620d0a9a5a0069bc894fcd92e1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 15:04:04 -0700 Subject: Document INCLUDE_DIRS --- makefiles/module.mk | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/makefiles/module.mk b/makefiles/module.mk index e2a1041e0..538f6d318 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -75,6 +75,12 @@ # the list should be formatted as: # ... # +# INCLUDE_DIRS (optional, must be appended) +# +# The list of directories searched for include files. If non-standard +# includes (e.g. those from another module) are required, paths to search +# can be added here. +# # DEFAULT_VISIBILITY (optional) # # If not set, global symbols defined in a module will not be visible -- cgit v1.2.3 From 4fe9e1c4472e3747df62d1808d727768ef3b3751 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 15:05:12 -0700 Subject: Avoid spurious += in INCLUDE_DIRS --- makefiles/nuttx.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index 5186dc3ef..346735a02 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -64,8 +64,8 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # Add directories from the NuttX export to the relevant search paths # INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ - += $(NUTTX_EXPORT_DIR)arch/chip \ - += $(NUTTX_EXPORT_DIR)arch/common + $(NUTTX_EXPORT_DIR)arch/chip \ + $(NUTTX_EXPORT_DIR)arch/common LIB_DIRS += $(NUTTX_EXPORT_DIR)libs LIBS += -lapps -lnuttx -- cgit v1.2.3 From c71f4cf8690a707cd7385a90d826d9e0a5a351e2 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 style build system --- apps/mavlink/.context | 0 apps/mavlink/.gitignore | 3 - apps/mavlink/Makefile | 44 - apps/mavlink/mavlink.c | 825 -------------- apps/mavlink/mavlink_bridge_header.h | 81 -- apps/mavlink/mavlink_hil.h | 61 -- apps/mavlink/mavlink_log.c | 89 -- apps/mavlink/mavlink_log.h | 120 --- apps/mavlink/mavlink_parameters.c | 231 ---- apps/mavlink/mavlink_parameters.h | 104 -- apps/mavlink/mavlink_receiver.c | 718 ------------- apps/mavlink/missionlib.c | 200 ---- apps/mavlink/missionlib.h | 52 - apps/mavlink/orb_listener.c | 777 -------------- apps/mavlink/orb_topics.h | 106 -- apps/mavlink/util.h | 54 - apps/mavlink/waypoints.c | 1134 -------------------- apps/mavlink/waypoints.h | 131 --- apps/mavlink_onboard/Makefile | 44 - apps/mavlink_onboard/mavlink.c | 529 --------- apps/mavlink_onboard/mavlink_bridge_header.h | 81 -- apps/mavlink_onboard/mavlink_receiver.c | 331 ------ apps/mavlink_onboard/orb_topics.h | 100 -- apps/mavlink_onboard/util.h | 54 - makefiles/config_px4fmu_default.mk | 4 +- makefiles/config_px4fmuv2_default.mk | 4 +- src/include/mavlink/mavlink_log.h | 120 +++ src/modules/mavlink/.context | 0 src/modules/mavlink/.gitignore | 3 + src/modules/mavlink/mavlink.c | 825 ++++++++++++++ src/modules/mavlink/mavlink_bridge_header.h | 81 ++ src/modules/mavlink/mavlink_hil.h | 61 ++ src/modules/mavlink/mavlink_log.c | 89 ++ src/modules/mavlink/mavlink_parameters.c | 231 ++++ src/modules/mavlink/mavlink_parameters.h | 104 ++ src/modules/mavlink/mavlink_receiver.c | 718 +++++++++++++ src/modules/mavlink/missionlib.c | 200 ++++ src/modules/mavlink/missionlib.h | 52 + src/modules/mavlink/module.mk | 47 + src/modules/mavlink/orb_listener.c | 778 ++++++++++++++ src/modules/mavlink/orb_topics.h | 106 ++ src/modules/mavlink/util.h | 54 + src/modules/mavlink/waypoints.c | 1134 ++++++++++++++++++++ src/modules/mavlink/waypoints.h | 131 +++ 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 + 50 files changed, 5875 insertions(+), 5873 deletions(-) delete mode 100644 apps/mavlink/.context delete mode 100644 apps/mavlink/.gitignore delete mode 100644 apps/mavlink/Makefile delete mode 100644 apps/mavlink/mavlink.c delete mode 100644 apps/mavlink/mavlink_bridge_header.h delete mode 100644 apps/mavlink/mavlink_hil.h delete mode 100644 apps/mavlink/mavlink_log.c delete mode 100644 apps/mavlink/mavlink_log.h delete mode 100644 apps/mavlink/mavlink_parameters.c delete mode 100644 apps/mavlink/mavlink_parameters.h delete mode 100644 apps/mavlink/mavlink_receiver.c delete mode 100644 apps/mavlink/missionlib.c delete mode 100644 apps/mavlink/missionlib.h delete mode 100644 apps/mavlink/orb_listener.c delete mode 100644 apps/mavlink/orb_topics.h delete mode 100644 apps/mavlink/util.h delete mode 100644 apps/mavlink/waypoints.c delete mode 100644 apps/mavlink/waypoints.h delete mode 100644 apps/mavlink_onboard/Makefile delete mode 100644 apps/mavlink_onboard/mavlink.c delete mode 100644 apps/mavlink_onboard/mavlink_bridge_header.h delete mode 100644 apps/mavlink_onboard/mavlink_receiver.c delete mode 100644 apps/mavlink_onboard/orb_topics.h delete mode 100644 apps/mavlink_onboard/util.h create mode 100644 src/include/mavlink/mavlink_log.h create mode 100644 src/modules/mavlink/.context create mode 100644 src/modules/mavlink/.gitignore create mode 100644 src/modules/mavlink/mavlink.c create mode 100644 src/modules/mavlink/mavlink_bridge_header.h create mode 100644 src/modules/mavlink/mavlink_hil.h create mode 100644 src/modules/mavlink/mavlink_log.c create mode 100644 src/modules/mavlink/mavlink_parameters.c create mode 100644 src/modules/mavlink/mavlink_parameters.h create mode 100644 src/modules/mavlink/mavlink_receiver.c create mode 100644 src/modules/mavlink/missionlib.c create mode 100644 src/modules/mavlink/missionlib.h create mode 100644 src/modules/mavlink/module.mk create mode 100644 src/modules/mavlink/orb_listener.c create mode 100644 src/modules/mavlink/orb_topics.h create mode 100644 src/modules/mavlink/util.h create mode 100644 src/modules/mavlink/waypoints.c create mode 100644 src/modules/mavlink/waypoints.h 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 diff --git a/apps/mavlink/.context b/apps/mavlink/.context deleted file mode 100644 index e69de29bb..000000000 diff --git a/apps/mavlink/.gitignore b/apps/mavlink/.gitignore deleted file mode 100644 index a02827195..000000000 --- a/apps/mavlink/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -include -mavlink-* -pymavlink-* diff --git a/apps/mavlink/Makefile b/apps/mavlink/Makefile deleted file mode 100644 index 8ddb69b71..000000000 --- a/apps/mavlink/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. -# -############################################################################ - -# -# Mavlink Application -# - -APPNAME = mavlink -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c deleted file mode 100644 index 644b779af..000000000 --- a/apps/mavlink/mavlink.c +++ /dev/null @@ -1,825 +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 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 "waypoints.h" -#include "mavlink_log.h" -#include "orb_topics.h" -#include "missionlib.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" - -/* define MAVLink specific parameters */ -PARAM_DEFINE_INT32(MAV_SYS_ID, 1); -PARAM_DEFINE_INT32(MAV_COMP_ID, 50); -PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); - -__EXPORT int mavlink_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; -static pthread_t uorb_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_FIXED_WING, - 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); - -/* Allocate storage space for waypoints */ -static mavlink_wpm_storage wpm_s; -mavlink_wpm_storage *wpm = &wpm_s; - -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 struct mavlink_logbuffer lb; - -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); -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - - - -int -set_hil_on_off(bool hil_enabled) -{ - int ret = OK; - - /* Enable HIL */ - if (hil_enabled && !mavlink_hil_enabled) { - - /* Advertise topics */ - pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - - /* sensore level hil */ - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - - mavlink_hil_enabled = true; - - /* ramp up some HIL-related subscriptions */ - unsigned hil_rate_interval; - - if (baudrate < 19200) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (baudrate < 38400) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (baudrate < 115200) { - /* 20 Hz */ - hil_rate_interval = 50; - - } else { - /* 200 Hz */ - hil_rate_interval = 5; - } - - orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); - } - - if (!hil_enabled && mavlink_hil_enabled) { - mavlink_hil_enabled = false; - orb_set_interval(mavlink_subs.spa_sub, 200); - - } else { - ret = ERROR; - } - - return ret; -} - -void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; - - /* set mode flags independent of system state */ - - /* HIL */ - if (v_status.flag_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_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; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - 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; - } - -} - - -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) -{ - int ret = OK; - - switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* sensor sub triggers scaled IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_HIGHRES_IMU: - /* sensor sub triggers highres IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_RAW_IMU: - /* sensor sub triggers RAW IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_ATTITUDE: - /* attitude sub triggers attitude */ - orb_set_interval(subs->att_sub, min_interval); - break; - - case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: - /* actuator_outputs triggers this message */ - orb_set_interval(subs->act_0_sub, min_interval); - orb_set_interval(subs->act_1_sub, min_interval); - orb_set_interval(subs->act_2_sub, min_interval); - orb_set_interval(subs->act_3_sub, min_interval); - orb_set_interval(subs->actuators_sub, min_interval); - break; - - case MAVLINK_MSG_ID_MANUAL_CONTROL: - /* manual_control_setpoint triggers this message */ - orb_set_interval(subs->man_control_sp_sub, min_interval); - break; - - case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs->debug_key_value, min_interval); - break; - - default: - /* not found */ - ret = ERROR; - break; - } - - return ret; -} - - -/**************************************************************************** - * MAVLink text message logger - ****************************************************************************/ - -static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations mavlink_fops = { - .ioctl = mavlink_dev_ioctl -}; - -static int -mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) -{ - static unsigned int total_counter = 0; - - switch (cmd) { - case (int)MAVLINK_IOC_SEND_TEXT_INFO: - case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: - case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; - return OK; - } - - default: - return ENOTTY; - } -} - -#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 - -/**************************************************************************** - * 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; - - /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != 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; - static param_t param_system_id; - static param_t param_component_id; - static 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"); - initialized = true; - } - - /* update system and component id */ - int32_t system_id; - param_get(param_system_id, &system_id); - - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - int32_t component_id; - param_get(param_component_id, &component_id); - - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - int32_t system_type; - param_get(param_system_type, &system_type); - - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; - } -} - -/** - * MAVLink Protocol main function. - */ -int mavlink_thread_main(int argc, char *argv[]) -{ - /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); - - int ch; - char *device_name = "/dev/ttyS1"; - baudrate = 57600; - - /* 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); - - /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); - - /* Initialize system properties */ - mavlink_update_system(); - - /* start the MAVLink receiver */ - receive_thread = receive_start(uart); - - /* start the ORB receiver */ - uorb_receive_thread = uorb_receive_start(); - - /* initialize waypoint manager */ - mavlink_wpm_init(wpm); - - /* all subscriptions are now active, set up initial guess about rate limits */ - if (baudrate >= 230400) { - /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); - /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - - } else if (baudrate >= 115200) { - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); - /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - - } else if (baudrate >= 57600) { - /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); - /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); - /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); - - } else { - /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); - /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); - /* 0.5 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); - /* 0.1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); - } - - 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(&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); - - /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); - - /* 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 quarter the time */ - usleep(25000); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); - - /* sleep quarter the time */ - usleep(25000); - - /* send parameters at 20 Hz (if queued for sending) */ - mavlink_pm_queued_send(); - - /* sleep quarter the time */ - usleep(25000); - - if (baudrate > 57600) { - mavlink_pm_queued_send(); - } - - /* sleep 10 ms */ - usleep(10000); - - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); - - if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); - } - } - - /* sleep 15 ms */ - usleep(15000); - } - - /* wait for threads to complete */ - pthread_join(receive_thread, NULL); - pthread_join(uorb_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_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", - 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); - printf("."); - } - - 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/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h deleted file mode 100644 index 270ec1727..000000000 --- a/apps/mavlink/mavlink_bridge_header.h +++ /dev/null @@ -1,81 +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 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 "v1.0/mavlink_types.h" -#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/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h deleted file mode 100644 index 8c7a5b514..000000000 --- a/apps/mavlink/mavlink_hil.h +++ /dev/null @@ -1,61 +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 mavlink_hil.h - * Hardware-in-the-loop simulation support. - */ - -#pragma once - -extern bool mavlink_hil_enabled; - -extern struct vehicle_global_position_s hil_global_pos; -extern struct vehicle_attitude_s hil_attitude; -extern struct sensor_combined_s hil_sensors; -extern struct vehicle_gps_position_s hil_gps; -extern orb_advert_t pub_hil_global_pos; -extern orb_advert_t pub_hil_attitude; -extern orb_advert_t pub_hil_sensors; -extern orb_advert_t pub_hil_gps; - -/** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ -extern int set_hil_on_off(bool hil_enabled); diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c deleted file mode 100644 index ed65fb883..000000000 --- a/apps/mavlink/mavlink_log.c +++ /dev/null @@ -1,89 +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 mavlink_log.c - * MAVLink text logging. - * - * @author Lorenz Meier - */ - -#include -#include - -#include "mavlink_log.h" - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) -{ - lb->size = size; - lb->start = 0; - lb->count = 0; - lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); -} - -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) -{ - return lb->count == (int)lb->size; -} - -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) -{ - return lb->count == 0; -} - -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) -{ - int end = (lb->start + lb->count) % lb->size; - memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); - - if (mavlink_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - - } else { - ++lb->count; - } -} - -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) -{ - if (!mavlink_logbuffer_is_empty(lb)) { - memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); - lb->start = (lb->start + 1) % lb->size; - --lb->count; - return 0; - - } else { - return 1; - } -} diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h deleted file mode 100644 index 233a76cb3..000000000 --- a/apps/mavlink/mavlink_log.h +++ /dev/null @@ -1,120 +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 mavlink_log.h - * MAVLink text logging. - * - * @author Lorenz Meier - */ - -#ifndef MAVLINK_LOG -#define MAVLINK_LOG - -/* - * IOCTL interface for sending log messages. - */ -#include - -/* - * The mavlink log device node; must be opened before messages - * can be logged. - */ -#define MAVLINK_LOG_DEVICE "/dev/mavlink" - -#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1) -#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2) -#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3) - -/** - * Send a mavlink emergency message. - * - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ -#ifdef __cplusplus -#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); -#else -#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); -#endif - -/** - * Send a mavlink critical message. - * - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ -#ifdef __cplusplus -#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); -#else -#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); -#endif - -/** - * Send a mavlink info message. - * - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ -#ifdef __cplusplus -#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); -#else -#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); -#endif - -struct mavlink_logmessage { - char text[51]; - unsigned char severity; -}; - -struct mavlink_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct mavlink_logmessage *elems; -}; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); - -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); - -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); - -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); - -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); - -#endif - diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c deleted file mode 100644 index 819f3441b..000000000 --- a/apps/mavlink/mavlink_parameters.c +++ /dev/null @@ -1,231 +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 mavlink_parameters.c - * MAVLink parameter protocol implementation (BSD-relicensed). - * - * @author Lorenz Meier - */ - -#include "mavlink_bridge_header.h" -#include -#include "mavlink_parameters.h" -#include -#include "math.h" /* isinf / isnan checks */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -extern mavlink_system_t mavlink_system; - -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); - -/** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ -static unsigned int mavlink_param_queue_index = 0; - -/** - * Callback for param interface. - */ -void mavlink_pm_callback(void *arg, param_t param); - -void mavlink_pm_callback(void *arg, param_t param) -{ - mavlink_pm_send_param(param); - usleep(*(unsigned int *)arg); -} - -void mavlink_pm_send_all_params(unsigned int delay) -{ - unsigned int dbuf = delay; - param_foreach(&mavlink_pm_callback, &dbuf, false); -} - -int mavlink_pm_queued_send() -{ - if (mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); - mavlink_param_queue_index++; - return 0; - - } else { - return 1; - } -} - -void mavlink_pm_start_queued_send() -{ - mavlink_param_queue_index = 0; -} - -int mavlink_pm_send_param_for_index(uint16_t index) -{ - return mavlink_pm_send_param(param_for_index(index)); -} - -int mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} - -int mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) return 1; - - /* buffers for param transmission */ - static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - static mavlink_message_t tx_msg; - - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; - - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; - - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; - - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } - - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ - - int ret; - - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; - } - - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - MAVLINK_COMM_0, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - ret = mavlink_missionlib_send_message(&tx_msg); - return ret; -} - -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[mavlink pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } - - } break; - } -} diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h deleted file mode 100644 index b1e38bcc8..000000000 --- a/apps/mavlink/mavlink_parameters.h +++ /dev/null @@ -1,104 +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 mavlink_parameters.h - * MAVLink parameter protocol definitions (BSD-relicensed). - * - * @author Lorenz Meier - */ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - - -#include -#include -#include - -/** - * Handle parameter related messages. - */ -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - -/** - * Send all parameters at once. - * - * This function blocks until all parameters have been sent. - * it delays each parameter by the passed amount of microseconds. - * - * @param delay The delay in us between sending all parameters. - */ -void mavlink_pm_send_all_params(unsigned int delay); - -/** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ -int mavlink_pm_send_param(param_t param); - -/** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ -int mavlink_pm_send_param_for_index(uint16_t index); - -/** - * Send one parameter identified by name. - * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. - */ -int mavlink_pm_send_param_for_name(const char *name); - -/** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ -int mavlink_pm_queued_send(void); - -/** - * Start sending the parameter queue. - * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() - */ -void mavlink_pm_start_queued_send(void); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c deleted file mode 100644 index 22c2fcdad..000000000 --- a/apps/mavlink/mavlink_receiver.c +++ /dev/null @@ -1,718 +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 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 "waypoints.h" -#include "mavlink_log.h" -#include "orb_topics.h" -#include "missionlib.h" -#include "mavlink_hil.h" -#include "mavlink_parameters.h" -#include "util.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; -struct vehicle_gps_position_s hil_gps; -struct sensor_combined_s hil_sensors; -orb_advert_t pub_hil_global_pos = -1; -orb_advert_t pub_hil_attitude = -1; -orb_advert_t pub_hil_gps = -1; -orb_advert_t pub_hil_sensors = -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 = flow.time_usec; - 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.timestamp = hrt_absolute_time(); - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.yaw; - - 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); - } - } - } - - /* - * Only decode hil messages in HIL mode. - * - * The HIL mode is enabled by the HIL bit flag - * in the system mode. Either send a set mode - * COMMAND_LONG message or a SET_MODE message - */ - - if (mavlink_hil_enabled) { - - uint64_t timestamp = hrt_absolute_time(); - - /* TODO, set ground_press/ temp during calib */ - static const float ground_press = 1013.25f; // mbar - static const float ground_tempC = 21.0f; - static const float ground_alt = 0.0f; - static const float T0 = 273.15; - static const float R = 287.05f; - static const float g = 9.806f; - - if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { - - mavlink_raw_imu_t imu; - mavlink_msg_raw_imu_decode(msg, &imu); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = imu.time_usec; - - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; - hil_sensors.gyro_raw[0] = imu.xgyro; - hil_sensors.gyro_raw[1] = imu.ygyro; - hil_sensors.gyro_raw[2] = imu.zgyro; - hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad; - hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad; - hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad; - - /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; - static const float mg2ms2 = 9.8f / 1000.0f; - hil_sensors.accelerometer_raw[0] = imu.xacc; - hil_sensors.accelerometer_raw[1] = imu.yacc; - hil_sensors.accelerometer_raw[2] = imu.zacc; - hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc; - hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc; - hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - - /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; - - /* magnetometer */ - float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.magnetometer_raw[0] = imu.xmag; - hil_sensors.magnetometer_raw[1] = imu.ymag; - hil_sensors.magnetometer_raw[2] = imu.zmag; - hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga; - hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga; - hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil imu at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - - if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { - - mavlink_highres_imu_t imu; - mavlink_msg_highres_imu_decode(msg, &imu); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = hrt_absolute_time(); - - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; - hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; - hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; - hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - - /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; - static const float mg2ms2 = 9.8f / 1000.0f; - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - - /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; - - /* magnetometer */ - float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; - hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; - hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; - hil_sensors.magnetometer_ga[0] = imu.xmag; - hil_sensors.magnetometer_ga[1] = imu.ymag; - hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - - hil_sensors.baro_pres_mbar = imu.abs_pressure; - - float tempC = imu.temperature; - float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; - float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); - - hil_sensors.baro_alt_meter = h; - hil_sensors.baro_temp_celcius = imu.temperature; - - hil_sensors.gyro_counter = hil_counter; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.accelerometer_counter = hil_counter; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter++; - hil_frames++; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil imu at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - - if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { - - mavlink_gps_raw_int_t gps; - mavlink_msg_gps_raw_int_decode(msg, &gps); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* gps */ - hil_gps.timestamp_position = gps.time_usec; - hil_gps.time_gps_usec = gps.time_usec; - hil_gps.lat = gps.lat; - hil_gps.lon = gps.lon; - hil_gps.alt = gps.alt; - hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m - hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; - hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - - /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ - float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; - /* go back to -PI..PI */ - if (heading_rad > M_PI_F) - heading_rad -= 2.0f * M_PI_F; - hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); - hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); - hil_gps.vel_d_m_s = 0.0f; - hil_gps.vel_ned_valid = true; - /* COG (course over ground) is speced as -PI..+PI */ - hil_gps.cog_rad = heading_rad; - hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; - - /* publish */ - orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil gps at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - - if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) { - - mavlink_raw_pressure_t press; - mavlink_msg_raw_pressure_decode(msg, &press); - - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - - /* sensors general */ - hil_sensors.timestamp = press.time_usec; - - /* baro */ - - float tempC = press.temperature / 100.0f; - float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; - float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs); - hil_sensors.baro_counter = hil_counter; - hil_sensors.baro_pres_mbar = press.press_abs; - hil_sensors.baro_alt_meter = h; - hil_sensors.baro_temp_celcius = tempC; - - /* publish */ - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - // increment counters - hil_counter += 1 ; - hil_frames += 1 ; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil pressure at %d hz\n", hil_frames/10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { - - mavlink_hil_state_t hil_state; - mavlink_msg_hil_state_decode(msg, &hil_state); - - /* Calculate Rotation Matrix */ - //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode - - if (mavlink_system.type == MAV_TYPE_FIXED_WING) { - //TODO: assuming low pitch and roll values for now - hil_attitude.R[0][0] = cosf(hil_state.yaw); - hil_attitude.R[0][1] = sinf(hil_state.yaw); - hil_attitude.R[0][2] = 0.0f; - - hil_attitude.R[1][0] = -sinf(hil_state.yaw); - hil_attitude.R[1][1] = cosf(hil_state.yaw); - hil_attitude.R[1][2] = 0.0f; - - hil_attitude.R[2][0] = 0.0f; - hil_attitude.R[2][1] = 0.0f; - hil_attitude.R[2][2] = 1.0f; - - hil_attitude.R_valid = true; - } - - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; - - - /* set timestamp and notify processes (broadcast) */ - hil_global_pos.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); - - hil_attitude.roll = hil_state.roll; - hil_attitude.pitch = hil_state.pitch; - hil_attitude.yaw = hil_state.yaw; - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - - /* set timestamp and notify processes (broadcast) */ - hil_attitude.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); - } - - if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - struct rc_channels_s rc_hil; - memset(&rc_hil, 0, sizeof(rc_hil)); - static orb_advert_t rc_pub = 0; - - rc_hil.timestamp = hrt_absolute_time(); - rc_hil.chan_count = 4; - - rc_hil.chan[0].scaled = man.x / 1000.0f; - rc_hil.chan[1].scaled = man.y / 1000.0f; - rc_hil.chan[2].scaled = man.r / 1000.0f; - rc_hil.chan[3].scaled = man.z / 1000.0f; - - struct manual_control_setpoint_s mc; - static orb_advert_t mc_pub = 0; - - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc); - - mc.timestamp = rc_hil.timestamp; - mc.roll = man.x / 1000.0f; - mc.pitch = man.y / 1000.0f; - mc.yaw = man.r / 1000.0f; - mc.throttle = man.z / 1000.0f; - - /* fake RC channels with manual control input from simulator */ - - - if (rc_pub == 0) { - rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); - - } else { - orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); - } - - if (mc_pub == 0) { - mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); - } - } - } -} - - -/** - * Receive data from UART. - */ -static void * -receive_thread(void *arg) -{ - int uart_fd = *((int *)arg); - - const int timeout = 1000; - uint8_t buf[32]; - - mavlink_message_t msg; - - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); - - while (!thread_should_exit) { - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - - if (poll(fds, 1, timeout) > 0) { - /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); - - /* if read failed, this loop won't execute */ - for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(chan, buf[i], &msg, &status)) { - /* handle generic messages and commands */ - handle_message(&msg); - - /* Handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - - /* Handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); - } - } - } - } - - return NULL; -} - -pthread_t -receive_start(int uart) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - // set to non-blocking read - int flags = fcntl(uart, F_GETFL, 0); - fcntl(uart, F_SETFL, flags | O_NONBLOCK); - - 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; -} diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c deleted file mode 100644 index 8064febc4..000000000 --- a/apps/mavlink/missionlib.c +++ /dev/null @@ -1,200 +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 missionlib.h - * MAVLink missionlib components - */ - -// 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 "waypoints.h" -#include "mavlink_log.h" -#include "orb_topics.h" -#include "missionlib.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -int -mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); - return 0; -} - -int -mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - return mavlink_missionlib_send_message(&msg); - - } else { - return 1; - } -} - -/** - * Get system time since boot in microseconds - * - * @return the system time since boot in microseconds - */ -uint64_t mavlink_missionlib_get_system_timestamp() -{ - return hrt_absolute_time(); -} - -/** - * This callback is executed each time a waypoint changes. - * - * It publishes the vehicle_global_position_setpoint_s or the - * vehicle_local_position_setpoint_s topic, depending on the type of waypoint - */ -void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) -{ - static orb_advert_t global_position_setpoint_pub = -1; - static orb_advert_t local_position_setpoint_pub = -1; - char buf[50] = {0}; - - /* Update controller setpoints */ - if (frame == (int)MAV_FRAME_GLOBAL) { - /* global, absolute waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - - sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - /* global, relative alt (in relation to HOME) waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = true; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - - sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { - /* local, absolute waypoint */ - struct vehicle_local_position_setpoint_s sp; - sp.x = param5_lat_x; - sp.y = param6_lon_y; - sp.z = param7_alt_z; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; - - /* Initialize publication if necessary */ - if (local_position_setpoint_pub < 0) { - local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); - } - - sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - } - - mavlink_missionlib_send_gcs_string(buf); - printf("%s\n", buf); - //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); -} diff --git a/apps/mavlink/missionlib.h b/apps/mavlink/missionlib.h deleted file mode 100644 index c2ca735b3..000000000 --- a/apps/mavlink/missionlib.h +++ /dev/null @@ -1,52 +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 missionlib.h - * MAVLink mission helper library - */ - -#pragma once - -#include - -//extern void mavlink_wpm_send_message(mavlink_message_t *msg); -//extern void mavlink_wpm_send_gcs_string(const char *string); -//extern uint64_t mavlink_wpm_get_system_timestamp(void); -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); -extern uint64_t mavlink_missionlib_get_system_timestamp(void); -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c deleted file mode 100644 index 5f15facf8..000000000 --- a/apps/mavlink/orb_listener.c +++ /dev/null @@ -1,777 +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 orb_listener.c - * Monitors ORB topics and sends update messages as appropriate. - * - * @author Lorenz Meier - */ - -// XXX trim includes -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include - -#include "waypoints.h" -#include "mavlink_log.h" -#include "orb_topics.h" -#include "missionlib.h" -#include "mavlink_hil.h" -#include "util.h" - -extern bool gcs_link; - -struct vehicle_global_position_s global_pos; -struct vehicle_local_position_s local_pos; -struct vehicle_status_s v_status; -struct rc_channels_s rc; -struct rc_input_values rc_raw; -struct actuator_armed_s armed; - -struct mavlink_subscriptions mavlink_subs; - -static int status_sub; -static int rc_sub; - -static unsigned int sensors_raw_counter; -static unsigned int attitude_counter; -static unsigned int gps_counter; - -/* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ -static uint64_t last_sensor_timestamp; - -static void *uorb_receive_thread(void *arg); - -struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; -}; - -static void l_sensor_combined(const struct listener *l); -static void l_vehicle_attitude(const struct listener *l); -static void l_vehicle_gps_position(const struct listener *l); -static void l_vehicle_status(const struct listener *l); -static void l_rc_channels(const struct listener *l); -static void l_input_rc(const struct listener *l); -static void l_global_position(const struct listener *l); -static void l_local_position(const struct listener *l); -static void l_global_position_setpoint(const struct listener *l); -static void l_local_position_setpoint(const struct listener *l); -static void l_attitude_setpoint(const struct listener *l); -static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); -static void l_manual_control_setpoint(const struct listener *l); -static void l_vehicle_attitude_controls(const struct listener *l); -static void l_debug_key_value(const struct listener *l); -static void l_optical_flow(const struct listener *l); -static void l_vehicle_rates_setpoint(const struct listener *l); -static void l_home(const struct listener *l); - -static const struct listener listeners[] = { - {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, - {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, - {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, - {l_vehicle_status, &status_sub, 0}, - {l_rc_channels, &rc_sub, 0}, - {l_input_rc, &mavlink_subs.input_rc_sub, 0}, - {l_global_position, &mavlink_subs.global_pos_sub, 0}, - {l_local_position, &mavlink_subs.local_pos_sub, 0}, - {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, - {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, - {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, - {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, - {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, - {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, - {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, - {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, - {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, - {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, - {l_optical_flow, &mavlink_subs.optical_flow, 0}, - {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, - {l_home, &mavlink_subs.home_sub, 0}, -}; - -static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); - -void -l_sensor_combined(const struct listener *l) -{ - struct sensor_combined_s raw; - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); - - last_sensor_timestamp = raw.timestamp; - - /* mark individual fields as changed */ - uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; - - if (accel_counter != raw.accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = raw.accelerometer_counter; - } - - if (gyro_counter != raw.gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; - } - - if (mag_counter != raw.magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; - } - - if (baro_counter != raw.baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; - } - - if (gcs_link) - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], - raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], - raw.gyro_rad_s[1], raw.gyro_rad_s[2], - raw.magnetometer_ga[0], - raw.magnetometer_ga[1], raw.magnetometer_ga[2], - raw.baro_pres_mbar, 0 /* no diff pressure yet */, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); - - sensors_raw_counter++; -} - -void -l_vehicle_attitude(const struct listener *l) -{ - struct vehicle_attitude_s att; - - - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - - if (gcs_link) - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); - - attitude_counter++; -} - -void -l_vehicle_gps_position(const struct listener *l) -{ - struct vehicle_gps_position_s gps; - - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); - - /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = gps.cog_rad; - if (cog_deg > M_PI_F) - cog_deg -= 2.0f * M_PI_F; - cog_deg *= M_RAD_TO_DEG_F; - - - /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - gps.eph_m * 1e2f, // from m to cm - gps.epv_m * 1e2f, // from m to cm - gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from rad to deg * 100 - gps.satellites_visible); - - /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, - gps.satellites_visible, - gps.satellite_prn, - gps.satellite_used, - gps.satellite_elevation, - gps.satellite_azimuth, - gps.satellite_snr); - } - - gps_counter++; -} - -void -l_vehicle_status(const struct listener *l) -{ - /* immediately communicate state changes back to user */ - orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - - /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&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); -} - -void -l_rc_channels(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - // XXX Add RC channels scaled message here -} - -void -l_input_rc(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); - - if (gcs_link) - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, - 0, - (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, - (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, - (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, - (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, - (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, - (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, - (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, - (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, - 255); -} - -void -l_global_position(const struct listener *l) -{ - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - - uint64_t timestamp = global_pos.timestamp; - int32_t lat = global_pos.lat; - int32_t lon = global_pos.lon; - int32_t alt = (int32_t)(global_pos.alt * 1000); - int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); - int16_t vx = (int16_t)(global_pos.vx * 100.0f); - int16_t vy = (int16_t)(global_pos.vy * 100.0f); - int16_t vz = (int16_t)(global_pos.vz * 100.0f); - - /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, - timestamp / 1000, - lat, - lon, - alt, - relative_alt, - vx, - vy, - vz, - hdg); -} - -void -l_local_position(const struct listener *l) -{ - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - - if (gcs_link) - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); -} - -void -l_global_position_setpoint(const struct listener *l) -{ - struct vehicle_global_position_setpoint_s global_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); - - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (global_sp.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - - if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - coordinate_frame, - global_sp.lat, - global_sp.lon, - global_sp.altitude, - global_sp.yaw); -} - -void -l_local_position_setpoint(const struct listener *l) -{ - struct vehicle_local_position_setpoint_s local_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); - - if (gcs_link) - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); -} - -void -l_attitude_setpoint(const struct listener *l) -{ - struct vehicle_attitude_setpoint_s att_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); - - if (gcs_link) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); -} - -void -l_vehicle_rates_setpoint(const struct listener *l) -{ - struct vehicle_rates_setpoint_s rates_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); - - if (gcs_link) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, - rates_sp.timestamp / 1000, - rates_sp.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); -} - -void -l_actuator_outputs(const struct listener *l) -{ - struct actuator_outputs_s act_outputs; - - orb_id_t ids[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - /* copy actuator data into local buffer */ - orb_copy(ids[l->arg], *l->subp, &act_outputs); - - if (gcs_link) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - l->arg /* port number */, - act_outputs.output[0], - act_outputs.output[1], - act_outputs.output[2], - act_outputs.output[3], - act_outputs.output[4], - act_outputs.output[5], - act_outputs.output[6], - act_outputs.output[7]); - - /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - - /* scale / assign outputs depending on system type */ - - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 500.0f, - (act_outputs.output[1] - 1500.0f) / 500.0f, - (act_outputs.output[2] - 1500.0f) / 500.0f, - (act_outputs.output[3] - 1000.0f) / 1000.0f, - (act_outputs.output[4] - 1500.0f) / 500.0f, - (act_outputs.output[5] - 1500.0f) / 500.0f, - (act_outputs.output[6] - 1500.0f) / 500.0f, - (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, - 0); - } - } - } -} - -void -l_actuator_armed(const struct listener *l) -{ - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); -} - -void -l_manual_control_setpoint(const struct listener *l) -{ - struct manual_control_setpoint_s man_control; - - /* copy manual control data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); - - if (gcs_link) - mavlink_msg_manual_control_send(MAVLINK_COMM_0, - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); -} - -void -l_vehicle_attitude_controls(const struct listener *l) -{ - struct actuator_controls_effective_s actuators; - - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); - - if (gcs_link) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl0 ", - actuators.control_effective[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl1 ", - actuators.control_effective[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl2 ", - actuators.control_effective[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "eff ctrl3 ", - actuators.control_effective[3]); - } -} - -void -l_debug_key_value(const struct listener *l) -{ - struct debug_key_value_s debug; - - orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); - - /* Enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - debug.key, - debug.value); -} - -void -l_optical_flow(const struct listener *l) -{ - struct optical_flow_s flow; - - orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); - - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); -} - -void -l_home(const struct listener *l) -{ - struct home_position_s home; - - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); -} - -static void * -uorb_receive_thread(void *arg) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; - - /* - * Initialise listener array. - * - * Might want to invoke each listener once to set initial state. - */ - struct pollfd fds[n_listeners]; - - for (unsigned i = 0; i < n_listeners; i++) { - fds[i].fd = *listeners[i].subp; - fds[i].events = POLLIN; - - /* Invoke callback to set initial state */ - //listeners[i].callback(&listener[i]); - } - - while (!thread_should_exit) { - - int poll_ret = poll(fds, n_listeners, timeout); - - /* handle the poll result */ - if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); - - } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); - - } else { - - for (unsigned i = 0; i < n_listeners; i++) { - if (fds[i].revents & POLLIN) - listeners[i].callback(&listeners[i]); - } - } - } - - return NULL; -} - -pthread_t -uorb_receive_start(void) -{ - /* --- SENSORS RAW VALUE --- */ - mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ - - /* --- ATTITUDE VALUE --- */ - mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ - - /* --- GPS VALUE --- */ - mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ - - /* --- HOME POSITION --- */ - mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); - orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ - - /* --- SYSTEM STATE --- */ - status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - - /* --- RC CHANNELS VALUE --- */ - rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ - - /* --- RC RAW VALUE --- */ - mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); - orb_set_interval(mavlink_subs.input_rc_sub, 100); - - /* --- GLOBAL POS VALUE --- */ - mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ - - /* --- LOCAL POS VALUE --- */ - mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ - - /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ - - /* --- LOCAL SETPOINT VALUE --- */ - mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ - - /* --- ATTITUDE SETPOINT VALUE --- */ - mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ - - /* --- RATES SETPOINT VALUE --- */ - mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ - - /* --- ACTUATOR OUTPUTS --- */ - mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ - - /* --- DEBUG VALUE OUTPUT --- */ - mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ - - /* --- FLOW SENSOR --- */ - mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); - orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ - - /* start the listener loop */ - pthread_attr_t uorb_attr; - pthread_attr_init(&uorb_attr); - - /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); - return thread; -} diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h deleted file mode 100644 index d61cd43dc..000000000 --- a/apps/mavlink/orb_topics.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * 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 -#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; - int optical_flow; - int rates_setpoint_sub; - int home_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/apps/mavlink/util.h b/apps/mavlink/util.h deleted file mode 100644 index a4ff06a88..000000000 --- a/apps/mavlink/util.h +++ /dev/null @@ -1,54 +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 util.h - * Utility and helper functions and data. - */ - -#pragma once - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** Waypoint storage */ -extern mavlink_wpm_storage *wpm; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c deleted file mode 100644 index a131b143b..000000000 --- a/apps/mavlink/waypoints.c +++ /dev/null @@ -1,1134 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - * 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 waypoints.c - * MAVLink waypoint protocol implementation (BSD-relicensed). - */ - -#include -#include -#include -#include - -#include "missionlib.h" -#include "waypoints.h" -#include "util.h" - -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif - -bool debug = false; -bool verbose = false; - - -#define MAVLINK_WPM_NO_PRINTF - -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - -void mavlink_wpm_init(mavlink_wpm_storage *state) -{ - // Set all waypoints to zero - - // Set count to zero - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - -} - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = wpm->current_partner_sysid; - wpa.target_component = wpm->current_partner_compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); - -#endif - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); - } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, - cur->param2, cur->param3, cur->param4, cur->x, - cur->y, cur->z, cur->frame, cur->command); - - wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = wpm->current_partner_sysid; - wpc.target_component = wpm->current_partner_compid; - wpc.count = count; - - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_message_t msg; - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - wp->target_system = wpm->current_partner_sysid; - wp->target_component = wpm->current_partner_compid; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = wpm->current_partner_sysid; - wpr.target_component = wpm->current_partner_compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm->size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm->size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - -/* - * Calculate distance in global frame. - * - * The distance calculation is based on the WGS84 geoid (GPS) - */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) -{ - //TODO: implement for z once altidude contoller is implemented - - static uint16_t counter; - -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; - double x_rad = lat / 180.0 * M_PI; - double y_rad = lon / 180.0 * M_PI; - - double d_lat = x_rad - current_x_rad; - double d_lon = y_rad - current_y_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; - - } else { - return -1.0f; - } - - counter++; - -} - -/* - * Calculate distance in local frame (NED) - */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - float dx = (cur->x - x); - float dy = (cur->y - y); - float dz = (cur->z - z); - - return sqrt(dx * dx + dy * dy + dz * dz); - - } else { - return -1.0f; - } -} - -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) -{ - static uint16_t counter; - - // Do not flood the precious wireless link with debug data - // if (wpm->size > 0 && counter % 10 == 0) { - // printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); - // } - - - if (wpm->current_active_wp_id < wpm->size) { - - float orbit = wpm->waypoints[wpm->current_active_wp_id].param2; - int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; - float dist = -1.0f; - - if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); - - } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); - - } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); - - } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { - /* Check if conditions of mission item are satisfied */ - // XXX TODO - } - - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - wpm->pos_reached = true; - - if (counter % 100 == 0) - printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } - } - - //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { - if (wpm->current_active_wp_id < wpm->size) { - mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); - - if (wpm->timestamp_firstinside_orbit == 0) { - // Announce that last waypoint was reached - printf("Reached waypoint %u for the first time \n", cur_wp->seq); - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm->timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) { - printf("Reached waypoint %u long enough \n", cur_wp->seq); - - if (cur_wp->autocontinue) { - cur_wp->current = 0; - - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated restart the waypoint list from the beginning - */ - wpm->current_active_wp_id = 0; - - } else { - if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) - wpm->current_active_wp_id++; - } - - // Fly to next waypoint - wpm->timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->waypoints[wpm->current_active_wp_id].current = true; - wpm->pos_reached = false; - wpm->yaw_reached = false; - printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); - } - } - } - - } else { - wpm->timestamp_lastoutside_orbit = now; - } - - counter++; -} - - -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) -{ - /* check for timed-out operations */ - if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); - -#endif - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_count = 0; - wpm->current_partner_sysid = 0; - wpm->current_partner_compid = 0; - wpm->current_wp_id = -1; - - if (wpm->size == 0) { - wpm->current_active_wp_id = -1; - } - } - - // Do NOT continously send the current WP, since we're not loosing messages - // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // } - - check_waypoints_reached(now, global_position , local_position); - - return OK; -} - - -void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) -{ - uint64_t now = mavlink_missionlib_get_system_timestamp(); - - switch (msg->msgid) { -// case MAVLINK_MSG_ID_ATTITUDE: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) -// { -// mavlink_attitude_t att; -// mavlink_msg_attitude_decode(msg, &att); -// float yaw_tolerance = wpm->accept_range_yaw; -// //compare current yaw -// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) -// { -// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) -// wpm->yaw_reached = true; -// } -// else if(att.yaw - yaw_tolerance < 0.0f) -// { -// float lowerBound = 360.0f + att.yaw - yaw_tolerance; -// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) -// wpm->yaw_reached = true; -// } -// else -// { -// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; -// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) -// wpm->yaw_reached = true; -// } -// } -// } -// break; -// } -// -// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// -// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) -// { -// mavlink_local_position_ned_t pos; -// mavlink_msg_local_position_ned_decode(msg, &pos); -// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); -// -// wpm->pos_reached = false; -// -// // compare current position (given in message) with current waypoint -// float orbit = wp->param1; -// -// float dist; -//// if (wp->param2 == 0) -//// { -//// // FIXME segment distance -//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -//// } -//// else -//// { -// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); -//// } -// -// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) -// { -// wpm->pos_reached = true; -// } -// } -// } -// break; -// } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm->size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpm->current_wp_id == wpm->size - 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - -#endif - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_wp_id = 0; - } - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < wpm->size) { - // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - wpm->current_active_wp_id = wpc.seq; - uint32_t i; - - for (i = 0; i < wpm->size; i++) { - if (i == wpm->current_active_wp_id) { - wpm->waypoints[i].current = true; - - } else { - wpm->waypoints[i].current = false; - } - } - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); - -#endif - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->size > 0) { - //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - - } else { - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - - wpm->current_count = wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); - - } else { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); - } - } else { - // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm->current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); - - } else { - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - break; - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - -#endif - } - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); - -#endif - - } else if (wpr.seq >= wpm->size) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); - -#endif - } - } - } - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); - -#endif - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { -// printf("wpc count in: %d\n",wpc.count); -// printf("Comp id: %d\n",msg->compid); -// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); - - if (wpc.count > 0) { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - -#endif - } - - wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - wpm->current_count = wpc.count; - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } - - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (wpc.count == 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("COUNT 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - break; - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CMD"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - -#endif - } - - } else { - if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); - -#endif - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); - -#endif - } - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - mavlink_missionlib_send_gcs_string("GOT WP"); -// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); -// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); - -// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) - if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { - - wpm->timestamp_lastaction = now; - -// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); - -// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { - //mavlink_missionlib_send_gcs_string("DEBUG 2"); - -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); -// - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); -// printf("WP seq: %d\n",wp.seq); - wpm->current_wp_id = wp.seq + 1; - - // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); -// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - -// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - mavlink_missionlib_send_gcs_string("GOT ALL WPS"); - // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - - if (wpm->current_active_wp_id > wpm->rcv_size - 1) { - wpm->current_active_wp_id = wpm->rcv_size - 1; - } - - // switch the waypoints list - // FIXME CHECK!!! - uint32_t i; - - for (i = 0; i < wpm->current_count; ++i) { - wpm->waypoints[i] = wpm->rcv_waypoints[i]; - } - - wpm->size = wpm->current_count; - - //get the new current waypoint - - for (i = 0; i < wpm->size; i++) { - if (wpm->waypoints[i].current == 1) { - wpm->current_active_wp_id = i; - //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == wpm->size) { - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - wpm->timestamp_firstinside_orbit = 0; - } - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - } else { - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - } - - } else { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); - break; - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (!(wp.seq == 0)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (!(wp.seq == wpm->current_wp_id)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (!(wp.seq < wpm->current_count)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - } - } else { - //we we're target but already communicating with someone else - if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); - } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; - - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm->size = 0; - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); - } - - break; - } - - default: { - // if (debug) // printf("Waypoint: received message of unknown type"); - break; - } - } - - check_waypoints_reached(now, global_pos, local_pos); -} diff --git a/apps/mavlink/waypoints.h b/apps/mavlink/waypoints.h deleted file mode 100644 index c32ab32e5..000000000 --- a/apps/mavlink/waypoints.h +++ /dev/null @@ -1,131 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - * 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 waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include - -#ifndef MAVLINK_SEND_UART_BYTES -#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) -#endif -extern mavlink_system_t mavlink_system; -#include -#include -#include -#include - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 15 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#ifndef MAVLINK_WPM_TEXT_FEEDBACK -#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text -#endif -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t size; - uint16_t max_size; - uint16_t rcv_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; - uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -void mavlink_wpm_init(mavlink_wpm_storage *state); -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); -void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , - struct vehicle_local_position_s *local_pos); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -#endif /* WAYPOINTS_H_ */ diff --git a/apps/mavlink_onboard/Makefile b/apps/mavlink_onboard/Makefile deleted file mode 100644 index 8b1cb9b2c..000000000 --- a/apps/mavlink_onboard/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. -# -############################################################################ - -# -# Mavlink Application -# - -APPNAME = mavlink_onboard -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c deleted file mode 100644 index 5a2685560..000000000 --- a/apps/mavlink_onboard/mavlink.c +++ /dev/null @@ -1,529 +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 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/apps/mavlink_onboard/mavlink_bridge_header.h b/apps/mavlink_onboard/mavlink_bridge_header.h deleted file mode 100644 index bf7c5354c..000000000 --- a/apps/mavlink_onboard/mavlink_bridge_header.h +++ /dev/null @@ -1,81 +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 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 "v1.0/mavlink_types.h" -#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/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c deleted file mode 100644 index 0acbea675..000000000 --- a/apps/mavlink_onboard/mavlink_receiver.c +++ /dev/null @@ -1,331 +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 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/apps/mavlink_onboard/orb_topics.h b/apps/mavlink_onboard/orb_topics.h deleted file mode 100644 index f18f56243..000000000 --- a/apps/mavlink_onboard/orb_topics.h +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * 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/apps/mavlink_onboard/util.h b/apps/mavlink_onboard/util.h deleted file mode 100644 index 38a4db372..000000000 --- a/apps/mavlink_onboard/util.h +++ /dev/null @@ -1,54 +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 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); diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 6c43377e3..ada6b7ab7 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -21,6 +21,8 @@ MODULES += systemcmds/eeprom # General system control # MODULES += modules/commander +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard # # Estimation modules (EKF / other filters) @@ -56,8 +58,6 @@ 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, mavlink, , 2048, mavlink_main ) \ - $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ $(call _B, mixer, , 4096, mixer_main ) \ $(call _B, mpu6000, , 4096, mpu6000_main ) \ $(call _B, ms5611, , 2048, ms5611_main ) \ diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index fd69baa29..9aa4ec3da 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -21,6 +21,8 @@ MODULES += systemcmds/ramtron # General system control # MODULES += modules/commander +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard # # Estimation modules (EKF / other filters) @@ -55,8 +57,6 @@ 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, mavlink, , 2048, mavlink_main ) \ - $(call _B, mavlink_onboard, , 2048, mavlink_onboard_main ) \ $(call _B, mixer, , 4096, mixer_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) \ diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h new file mode 100644 index 000000000..233a76cb3 --- /dev/null +++ b/src/include/mavlink/mavlink_log.h @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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_log.h + * MAVLink text logging. + * + * @author Lorenz Meier + */ + +#ifndef MAVLINK_LOG +#define MAVLINK_LOG + +/* + * IOCTL interface for sending log messages. + */ +#include + +/* + * The mavlink log device node; must be opened before messages + * can be logged. + */ +#define MAVLINK_LOG_DEVICE "/dev/mavlink" + +#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1) +#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2) +#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3) + +/** + * Send a mavlink emergency message. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#ifdef __cplusplus +#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#else +#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#endif + +/** + * Send a mavlink critical message. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#ifdef __cplusplus +#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#else +#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#endif + +/** + * Send a mavlink info message. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#ifdef __cplusplus +#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#else +#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#endif + +struct mavlink_logmessage { + char text[51]; + unsigned char severity; +}; + +struct mavlink_logbuffer { + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct mavlink_logmessage *elems; +}; + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); + +#endif + diff --git a/src/modules/mavlink/.context b/src/modules/mavlink/.context new file mode 100644 index 000000000..e69de29bb diff --git a/src/modules/mavlink/.gitignore b/src/modules/mavlink/.gitignore new file mode 100644 index 000000000..a02827195 --- /dev/null +++ b/src/modules/mavlink/.gitignore @@ -0,0 +1,3 @@ +include +mavlink-* +pymavlink-* diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c new file mode 100644 index 000000000..de78cd139 --- /dev/null +++ b/src/modules/mavlink/mavlink.c @@ -0,0 +1,825 @@ +/**************************************************************************** + * + * 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 + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" + +/* define MAVLink specific parameters */ +PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); + +__EXPORT int mavlink_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; +static pthread_t uorb_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_FIXED_WING, + 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); + +/* Allocate storage space for waypoints */ +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; + +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 struct mavlink_logbuffer lb; + +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); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + + + +int +set_hil_on_off(bool hil_enabled) +{ + int ret = OK; + + /* Enable HIL */ + if (hil_enabled && !mavlink_hil_enabled) { + + /* Advertise topics */ + pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + + /* sensore level hil */ + pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + + mavlink_hil_enabled = true; + + /* ramp up some HIL-related subscriptions */ + unsigned hil_rate_interval; + + if (baudrate < 19200) { + /* 10 Hz */ + hil_rate_interval = 100; + + } else if (baudrate < 38400) { + /* 10 Hz */ + hil_rate_interval = 100; + + } else if (baudrate < 115200) { + /* 20 Hz */ + hil_rate_interval = 50; + + } else { + /* 200 Hz */ + hil_rate_interval = 5; + } + + orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + } + + if (!hil_enabled && mavlink_hil_enabled) { + mavlink_hil_enabled = false; + orb_set_interval(mavlink_subs.spa_sub, 200); + + } else { + ret = ERROR; + } + + return ret; +} + +void +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +{ + /* reset MAVLink mode bitfield */ + *mavlink_mode = 0; + + /* set mode flags independent of system state */ + + /* HIL */ + if (v_status.flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* manual input */ + if (v_status.flag_control_manual_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + /* attitude or rate control */ + if (v_status.flag_control_attitude_enabled || + v_status.flag_control_rates_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + /* vector control */ + if (v_status.flag_control_velocity_enabled || + v_status.flag_control_position_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + + /* autonomous mode */ + if (v_status.state_machine == SYSTEM_STATE_AUTO) { + *mavlink_mode |= MAV_MODE_FLAG_AUTO_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; + break; + + case SYSTEM_STATE_STABILIZED: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_AUTO: + *mavlink_state = MAV_STATE_ACTIVE; + 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; + } + +} + + +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +{ + int ret = OK; + + switch (mavlink_msg_id) { + case MAVLINK_MSG_ID_SCALED_IMU: + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_HIGHRES_IMU: + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_RAW_IMU: + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_ATTITUDE: + /* attitude sub triggers attitude */ + orb_set_interval(subs->att_sub, min_interval); + break; + + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + orb_set_interval(subs->act_0_sub, min_interval); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + orb_set_interval(subs->act_3_sub, min_interval); + orb_set_interval(subs->actuators_sub, min_interval); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + orb_set_interval(subs->debug_key_value, min_interval); + break; + + default: + /* not found */ + ret = ERROR; + break; + } + + return ret; +} + + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + +static const struct file_operations mavlink_fops = { + .ioctl = mavlink_dev_ioctl +}; + +static int +mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + static unsigned int total_counter = 0; + + switch (cmd) { + case (int)MAVLINK_IOC_SEND_TEXT_INFO: + case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: + case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { + const char *txt = (const char *)arg; + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); + total_counter++; + return OK; + } + + default: + return ENOTTY; + } +} + +#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 + +/**************************************************************************** + * 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; + + /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != 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; + static param_t param_system_id; + static param_t param_component_id; + static 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"); + initialized = true; + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + +/** + * MAVLink Protocol main function. + */ +int mavlink_thread_main(int argc, char *argv[]) +{ + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 5); + + int ch; + char *device_name = "/dev/ttyS1"; + baudrate = 57600; + + /* 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); + + /* create the device node that's used for sending text log messages, etc. */ + register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); + + /* start the ORB receiver */ + uorb_receive_thread = uorb_receive_start(); + + /* initialize waypoint manager */ + mavlink_wpm_init(wpm); + + /* all subscriptions are now active, set up initial guess about rate limits */ + if (baudrate >= 230400) { + /* 200 Hz / 5 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + + } else if (baudrate >= 115200) { + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + + } else if (baudrate >= 57600) { + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + /* 10 Hz / 100 ms ATTITUDE */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + + } else { + /* very low baud rate, limit to 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + /* 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + /* 0.5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + /* 0.1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + } + + 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(&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); + + /* switch HIL mode if required */ + set_hil_on_off(v_status.flag_hil_enabled); + + /* 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 quarter the time */ + usleep(25000); + + /* check if waypoint has been reached against the last positions */ + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + + /* sleep quarter the time */ + usleep(25000); + + /* send parameters at 20 Hz (if queued for sending) */ + mavlink_pm_queued_send(); + + /* sleep quarter the time */ + usleep(25000); + + if (baudrate > 57600) { + mavlink_pm_queued_send(); + } + + /* sleep 10 ms */ + usleep(10000); + + /* send one string at 10 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } + + /* sleep 15 ms */ + usleep(15000); + } + + /* wait for threads to complete */ + pthread_join(receive_thread, NULL); + pthread_join(uorb_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_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", + 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); + printf("."); + } + + 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/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h new file mode 100644 index 000000000..0010bb341 --- /dev/null +++ b/src/modules/mavlink/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/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h new file mode 100644 index 000000000..8c7a5b514 --- /dev/null +++ b/src/modules/mavlink/mavlink_hil.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 mavlink_hil.h + * Hardware-in-the-loop simulation support. + */ + +#pragma once + +extern bool mavlink_hil_enabled; + +extern struct vehicle_global_position_s hil_global_pos; +extern struct vehicle_attitude_s hil_attitude; +extern struct sensor_combined_s hil_sensors; +extern struct vehicle_gps_position_s hil_gps; +extern orb_advert_t pub_hil_global_pos; +extern orb_advert_t pub_hil_attitude; +extern orb_advert_t pub_hil_sensors; +extern orb_advert_t pub_hil_gps; + +/** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ +extern int set_hil_on_off(bool hil_enabled); diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c new file mode 100644 index 000000000..fa974dc0b --- /dev/null +++ b/src/modules/mavlink/mavlink_log.c @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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_log.c + * MAVLink text logging. + * + * @author Lorenz Meier + */ + +#include +#include + +#include + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +{ + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +{ + return lb->count == (int)lb->size; +} + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +{ + return lb->count == 0; +} + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +{ + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +{ + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + + } else { + return 1; + } +} diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c new file mode 100644 index 000000000..819f3441b --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.c @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * 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_parameters.c + * MAVLink parameter protocol implementation (BSD-relicensed). + * + * @author Lorenz Meier + */ + +#include "mavlink_bridge_header.h" +#include +#include "mavlink_parameters.h" +#include +#include "math.h" /* isinf / isnan checks */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern mavlink_system_t mavlink_system; + +extern int mavlink_missionlib_send_message(mavlink_message_t *msg); +extern int mavlink_missionlib_send_gcs_string(const char *string); + +/** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ +static unsigned int mavlink_param_queue_index = 0; + +/** + * Callback for param interface. + */ +void mavlink_pm_callback(void *arg, param_t param); + +void mavlink_pm_callback(void *arg, param_t param) +{ + mavlink_pm_send_param(param); + usleep(*(unsigned int *)arg); +} + +void mavlink_pm_send_all_params(unsigned int delay) +{ + unsigned int dbuf = delay; + param_foreach(&mavlink_pm_callback, &dbuf, false); +} + +int mavlink_pm_queued_send() +{ + if (mavlink_param_queue_index < param_count()) { + mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); + mavlink_param_queue_index++; + return 0; + + } else { + return 1; + } +} + +void mavlink_pm_start_queued_send() +{ + mavlink_param_queue_index = 0; +} + +int mavlink_pm_send_param_for_index(uint16_t index) +{ + return mavlink_pm_send_param(param_for_index(index)); +} + +int mavlink_pm_send_param_for_name(const char *name) +{ + return mavlink_pm_send_param(param_find(name)); +} + +int mavlink_pm_send_param(param_t param) +{ + if (param == PARAM_INVALID) return 1; + + /* buffers for param transmission */ + static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + float val_buf; + static mavlink_message_t tx_msg; + + /* query parameter type */ + param_type_t type = param_type(param); + /* copy parameter name */ + strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + uint8_t mavlink_type; + + if (type == PARAM_TYPE_INT32) { + mavlink_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + mavlink_type = MAVLINK_TYPE_FLOAT; + + } else { + mavlink_type = MAVLINK_TYPE_FLOAT; + } + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + + int ret; + + if ((ret = param_get(param, &val_buf)) != OK) { + return ret; + } + + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + name_buf, + val_buf, + mavlink_type, + param_count(), + param_get_index(param)); + ret = mavlink_missionlib_send_message(&tx_msg); + return ret; +} + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } break; + + case MAVLINK_MSG_ID_PARAM_SET: { + + /* Handle parameter setting */ + + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t mavlink_param_set; + mavlink_msg_param_set_decode(msg, &mavlink_param_set); + + if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[mavlink pm] unknown: %s", name); + mavlink_missionlib_send_gcs_string(buf); + + } else { + /* set and send parameter */ + param_set(param, &(mavlink_param_set.param_value)); + mavlink_pm_send_param(param); + } + } + } + } break; + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + mavlink_param_request_read_t mavlink_param_request_read; + mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); + + if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + /* when no index is given, loop through string ids and compare them */ + if (mavlink_param_request_read.param_index == -1) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + mavlink_pm_send_param_for_name(name); + + } else { + /* when index is >= 0, send this parameter again */ + mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); + } + } + + } break; + } +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 000000000..b1e38bcc8 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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_parameters.h + * MAVLink parameter protocol definitions (BSD-relicensed). + * + * @author Lorenz Meier + */ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + + +#include +#include +#include + +/** + * Handle parameter related messages. + */ +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + +/** + * Send all parameters at once. + * + * This function blocks until all parameters have been sent. + * it delays each parameter by the passed amount of microseconds. + * + * @param delay The delay in us between sending all parameters. + */ +void mavlink_pm_send_all_params(unsigned int delay); + +/** + * Send one parameter. + * + * @param param The parameter id to send. + * @return zero on success, nonzero on failure. + */ +int mavlink_pm_send_param(param_t param); + +/** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ +int mavlink_pm_send_param_for_index(uint16_t index); + +/** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ +int mavlink_pm_send_param_for_name(const char *name); + +/** + * Send a queue of parameters, one parameter per function call. + * + * @return zero on success, nonzero on failure + */ +int mavlink_pm_queued_send(void); + +/** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ +void mavlink_pm_start_queued_send(void); diff --git a/src/modules/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c new file mode 100644 index 000000000..e62e4dcc4 --- /dev/null +++ b/src/modules/mavlink/mavlink_receiver.c @@ -0,0 +1,718 @@ +/**************************************************************************** + * + * 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 + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "mavlink_parameters.h" +#include "util.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; +struct vehicle_gps_position_s hil_gps; +struct sensor_combined_s hil_sensors; +orb_advert_t pub_hil_global_pos = -1; +orb_advert_t pub_hil_attitude = -1; +orb_advert_t pub_hil_gps = -1; +orb_advert_t pub_hil_sensors = -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 = flow.time_usec; + 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.timestamp = hrt_absolute_time(); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + vicon_position.roll = pos.roll; + vicon_position.pitch = pos.pitch; + vicon_position.yaw = pos.yaw; + + 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); + } + } + } + + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + */ + + if (mavlink_hil_enabled) { + + uint64_t timestamp = hrt_absolute_time(); + + /* TODO, set ground_press/ temp during calib */ + static const float ground_press = 1013.25f; // mbar + static const float ground_tempC = 21.0f; + static const float ground_alt = 0.0f; + static const float T0 = 273.15f; + static const float R = 287.05f; + static const float g = 9.806f; + + if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { + + mavlink_raw_imu_t imu; + mavlink_msg_raw_imu_decode(msg, &imu); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = imu.time_usec; + + /* hil gyro */ + static const float mrad2rad = 1.0e-3f; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.gyro_raw[0] = imu.xgyro; + hil_sensors.gyro_raw[1] = imu.ygyro; + hil_sensors.gyro_raw[2] = imu.zgyro; + hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad; + hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad; + hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad; + + /* accelerometer */ + hil_sensors.accelerometer_counter = hil_counter; + static const float mg2ms2 = 9.8f / 1000.0f; + hil_sensors.accelerometer_raw[0] = imu.xacc; + hil_sensors.accelerometer_raw[1] = imu.yacc; + hil_sensors.accelerometer_raw[2] = imu.zacc; + hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc; + hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc; + hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + + /* adc */ + hil_sensors.adc_voltage_v[0] = 0; + hil_sensors.adc_voltage_v[1] = 0; + hil_sensors.adc_voltage_v[2] = 0; + + /* magnetometer */ + float mga2ga = 1.0e-3f; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_raw[0] = imu.xmag; + hil_sensors.magnetometer_raw[1] = imu.ymag; + hil_sensors.magnetometer_raw[2] = imu.zmag; + hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga; + hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga; + hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { + + mavlink_highres_imu_t imu; + mavlink_msg_highres_imu_decode(msg, &imu); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = hrt_absolute_time(); + + /* hil gyro */ + static const float mrad2rad = 1.0e-3f; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; + hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; + hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + + /* accelerometer */ + hil_sensors.accelerometer_counter = hil_counter; + static const float mg2ms2 = 9.8f / 1000.0f; + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + + /* adc */ + hil_sensors.adc_voltage_v[0] = 0; + hil_sensors.adc_voltage_v[1] = 0; + hil_sensors.adc_voltage_v[2] = 0; + + /* magnetometer */ + float mga2ga = 1.0e-3f; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; + hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; + hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + + hil_sensors.baro_pres_mbar = imu.abs_pressure; + + float tempC = imu.temperature; + float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; + float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); + + hil_sensors.baro_alt_meter = h; + hil_sensors.baro_temp_celcius = imu.temperature; + + hil_sensors.gyro_counter = hil_counter; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.accelerometer_counter = hil_counter; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter++; + hil_frames++; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { + + mavlink_gps_raw_int_t gps; + mavlink_msg_gps_raw_int_decode(msg, &gps); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* gps */ + hil_gps.timestamp_position = gps.time_usec; + hil_gps.time_gps_usec = gps.time_usec; + hil_gps.lat = gps.lat; + hil_gps.lon = gps.lon; + hil_gps.alt = gps.alt; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.s_variance_m_s = 5.0f; + hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + + /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ + float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ + if (heading_rad > M_PI_F) + heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); + hil_gps.vel_d_m_s = 0.0f; + hil_gps.vel_ned_valid = true; + /* COG (course over ground) is speced as -PI..+PI */ + hil_gps.cog_rad = heading_rad; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; + + /* publish */ + orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil gps at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) { + + mavlink_raw_pressure_t press; + mavlink_msg_raw_pressure_decode(msg, &press); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = press.time_usec; + + /* baro */ + + float tempC = press.temperature / 100.0f; + float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; + float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs); + hil_sensors.baro_counter = hil_counter; + hil_sensors.baro_pres_mbar = press.press_abs; + hil_sensors.baro_alt_meter = h; + hil_sensors.baro_temp_celcius = tempC; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil pressure at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { + + mavlink_hil_state_t hil_state; + mavlink_msg_hil_state_decode(msg, &hil_state); + + /* Calculate Rotation Matrix */ + //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode + + if (mavlink_system.type == MAV_TYPE_FIXED_WING) { + //TODO: assuming low pitch and roll values for now + hil_attitude.R[0][0] = cosf(hil_state.yaw); + hil_attitude.R[0][1] = sinf(hil_state.yaw); + hil_attitude.R[0][2] = 0.0f; + + hil_attitude.R[1][0] = -sinf(hil_state.yaw); + hil_attitude.R[1][1] = cosf(hil_state.yaw); + hil_attitude.R[1][2] = 0.0f; + + hil_attitude.R[2][0] = 0.0f; + hil_attitude.R[2][1] = 0.0f; + hil_attitude.R[2][2] = 1.0f; + + hil_attitude.R_valid = true; + } + + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; + + + /* set timestamp and notify processes (broadcast) */ + hil_global_pos.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + + hil_attitude.roll = hil_state.roll; + hil_attitude.pitch = hil_state.pitch; + hil_attitude.yaw = hil_state.yaw; + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + /* set timestamp and notify processes (broadcast) */ + hil_attitude.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } + + if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + struct rc_channels_s rc_hil; + memset(&rc_hil, 0, sizeof(rc_hil)); + static orb_advert_t rc_pub = 0; + + rc_hil.timestamp = hrt_absolute_time(); + rc_hil.chan_count = 4; + + rc_hil.chan[0].scaled = man.x / 1000.0f; + rc_hil.chan[1].scaled = man.y / 1000.0f; + rc_hil.chan[2].scaled = man.r / 1000.0f; + rc_hil.chan[3].scaled = man.z / 1000.0f; + + struct manual_control_setpoint_s mc; + static orb_advert_t mc_pub = 0; + + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* get a copy first, to prevent altering values that are not sent by the mavlink command */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc); + + mc.timestamp = rc_hil.timestamp; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.throttle = man.z / 1000.0f; + + /* fake RC channels with manual control input from simulator */ + + + if (rc_pub == 0) { + rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); + + } else { + orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); + } + + if (mc_pub == 0) { + mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); + } + } + } +} + + +/** + * Receive data from UART. + */ +static void * +receive_thread(void *arg) +{ + int uart_fd = *((int *)arg); + + const int timeout = 1000; + uint8_t buf[32]; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + + while (!thread_should_exit) { + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read. read may return negative values */ + ssize_t nread = read(uart_fd, buf, sizeof(buf)); + + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { + /* handle generic messages and commands */ + handle_message(&msg); + + /* Handle packet with waypoint component */ + mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + + /* Handle packet with parameter component */ + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + } + } + } + } + + return NULL; +} + +pthread_t +receive_start(int uart) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + // set to non-blocking read + int flags = fcntl(uart, F_GETFL, 0); + fcntl(uart, F_SETFL, flags | O_NONBLOCK); + + 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; +} diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c new file mode 100644 index 000000000..d369e05ff --- /dev/null +++ b/src/modules/mavlink/missionlib.c @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * 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 missionlib.h + * MAVLink missionlib components + */ + +// 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 + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" + +static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + +int +mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + + mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); + return 0; +} + +int +mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + return mavlink_missionlib_send_message(&msg); + + } else { + return 1; + } +} + +/** + * Get system time since boot in microseconds + * + * @return the system time since boot in microseconds + */ +uint64_t mavlink_missionlib_get_system_timestamp() +{ + return hrt_absolute_time(); +} + +/** + * This callback is executed each time a waypoint changes. + * + * It publishes the vehicle_global_position_setpoint_s or the + * vehicle_local_position_setpoint_s topic, depending on the type of waypoint + */ +void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) +{ + static orb_advert_t global_position_setpoint_pub = -1; + static orb_advert_t local_position_setpoint_pub = -1; + char buf[50] = {0}; + + /* Update controller setpoints */ + if (frame == (int)MAV_FRAME_GLOBAL) { + /* global, absolute waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = false; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + + sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + + } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + /* global, relative alt (in relation to HOME) waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = true; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + + sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + + } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { + /* local, absolute waypoint */ + struct vehicle_local_position_setpoint_s sp; + sp.x = param5_lat_x; + sp.y = param6_lon_y; + sp.z = param7_alt_z; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + + /* Initialize publication if necessary */ + if (local_position_setpoint_pub < 0) { + local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); + + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); + } + + sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + } + + mavlink_missionlib_send_gcs_string(buf); + printf("%s\n", buf); + //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); +} diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h new file mode 100644 index 000000000..c2ca735b3 --- /dev/null +++ b/src/modules/mavlink/missionlib.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 missionlib.h + * MAVLink mission helper library + */ + +#pragma once + +#include + +//extern void mavlink_wpm_send_message(mavlink_message_t *msg); +//extern void mavlink_wpm_send_gcs_string(const char *string); +//extern uint64_t mavlink_wpm_get_system_timestamp(void); +extern int mavlink_missionlib_send_message(mavlink_message_t *msg); +extern int mavlink_missionlib_send_gcs_string(const char *string); +extern uint64_t mavlink_missionlib_get_system_timestamp(void); +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk new file mode 100644 index 000000000..cbf08aeb2 --- /dev/null +++ b/src/modules/mavlink/module.mk @@ -0,0 +1,47 @@ +############################################################################ +# +# 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 +# + +MODULE_COMMAND = mavlink +SRCS += mavlink.c \ + missionlib.c \ + mavlink_parameters.c \ + mavlink_log.c \ + mavlink_receiver.c \ + orb_listener.c \ + waypoints.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c new file mode 100644 index 000000000..295cd5e28 --- /dev/null +++ b/src/modules/mavlink/orb_listener.c @@ -0,0 +1,778 @@ +/**************************************************************************** + * + * 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 orb_listener.c + * Monitors ORB topics and sends update messages as appropriate. + * + * @author Lorenz Meier + */ + +// XXX trim includes +#include +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" + +extern bool gcs_link; + +struct vehicle_global_position_s global_pos; +struct vehicle_local_position_s local_pos; +struct vehicle_status_s v_status; +struct rc_channels_s rc; +struct rc_input_values rc_raw; +struct actuator_armed_s armed; + +struct mavlink_subscriptions mavlink_subs; + +static int status_sub; +static int rc_sub; + +static unsigned int sensors_raw_counter; +static unsigned int attitude_counter; +static unsigned int gps_counter; + +/* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ +static uint64_t last_sensor_timestamp; + +static void *uorb_receive_thread(void *arg); + +struct listener { + void (*callback)(const struct listener *l); + int *subp; + uintptr_t arg; +}; + +static void l_sensor_combined(const struct listener *l); +static void l_vehicle_attitude(const struct listener *l); +static void l_vehicle_gps_position(const struct listener *l); +static void l_vehicle_status(const struct listener *l); +static void l_rc_channels(const struct listener *l); +static void l_input_rc(const struct listener *l); +static void l_global_position(const struct listener *l); +static void l_local_position(const struct listener *l); +static void l_global_position_setpoint(const struct listener *l); +static void l_local_position_setpoint(const struct listener *l); +static void l_attitude_setpoint(const struct listener *l); +static void l_actuator_outputs(const struct listener *l); +static void l_actuator_armed(const struct listener *l); +static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls(const struct listener *l); +static void l_debug_key_value(const struct listener *l); +static void l_optical_flow(const struct listener *l); +static void l_vehicle_rates_setpoint(const struct listener *l); +static void l_home(const struct listener *l); + +static const struct listener listeners[] = { + {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, + {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, + {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, + {l_vehicle_status, &status_sub, 0}, + {l_rc_channels, &rc_sub, 0}, + {l_input_rc, &mavlink_subs.input_rc_sub, 0}, + {l_global_position, &mavlink_subs.global_pos_sub, 0}, + {l_local_position, &mavlink_subs.local_pos_sub, 0}, + {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, + {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, + {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, + {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, + {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, + {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, + {l_optical_flow, &mavlink_subs.optical_flow, 0}, + {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, + {l_home, &mavlink_subs.home_sub, 0}, +}; + +static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); + +void +l_sensor_combined(const struct listener *l) +{ + struct sensor_combined_s raw; + + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); + + last_sensor_timestamp = raw.timestamp; + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = raw.accelerometer_counter; + } + + if (gyro_counter != raw.gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = raw.gyro_counter; + } + + if (mag_counter != raw.magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = raw.magnetometer_counter; + } + + if (baro_counter != raw.baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = raw.baro_counter; + } + + if (gcs_link) + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], + raw.gyro_rad_s[1], raw.gyro_rad_s[2], + raw.magnetometer_ga[0], + raw.magnetometer_ga[1], raw.magnetometer_ga[2], + raw.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); + + sensors_raw_counter++; +} + +void +l_vehicle_attitude(const struct listener *l) +{ + struct vehicle_attitude_s att; + + + /* copy attitude data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); + + if (gcs_link) + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); + + attitude_counter++; +} + +void +l_vehicle_gps_position(const struct listener *l) +{ + struct vehicle_gps_position_s gps; + + /* copy gps data into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + + /* GPS COG is 0..2PI in degrees * 1e2 */ + float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) + cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; + + + /* GPS position */ + mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + gps.eph_m * 1e2f, // from m to cm + gps.epv_m * 1e2f, // from m to cm + gps.vel_m_s * 1e2f, // from m/s to cm/s + cog_deg * 1e2f, // from rad to deg * 100 + gps.satellites_visible); + + /* update SAT info every 10 seconds */ + if (gps.satellite_info_available && (gps_counter % 50 == 0)) { + mavlink_msg_gps_status_send(MAVLINK_COMM_0, + gps.satellites_visible, + gps.satellite_prn, + gps.satellite_used, + gps.satellite_elevation, + gps.satellite_azimuth, + gps.satellite_snr); + } + + gps_counter++; +} + +void +l_vehicle_status(const struct listener *l) +{ + /* immediately communicate state changes back to user */ + orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + + /* enable or disable HIL */ + set_hil_on_off(v_status.flag_hil_enabled); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&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); +} + +void +l_rc_channels(const struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +l_input_rc(const struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); + + if (gcs_link) + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, + rc_raw.timestamp / 1000, + 0, + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); +} + +void +l_global_position(const struct listener *l) +{ + /* copy global position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); + + uint64_t timestamp = global_pos.timestamp; + int32_t lat = global_pos.lat; + int32_t lon = global_pos.lon; + int32_t alt = (int32_t)(global_pos.alt * 1000); + int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); + int16_t vx = (int16_t)(global_pos.vx * 100.0f); + int16_t vy = (int16_t)(global_pos.vy * 100.0f); + int16_t vz = (int16_t)(global_pos.vz * 100.0f); + + /* heading in degrees * 10, from 0 to 36.000) */ + uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + + mavlink_msg_global_position_int_send(MAVLINK_COMM_0, + timestamp / 1000, + lat, + lon, + alt, + relative_alt, + vx, + vy, + vz, + hdg); +} + +void +l_local_position(const struct listener *l) +{ + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); + + if (gcs_link) + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + local_pos.timestamp / 1000, + local_pos.x, + local_pos.y, + local_pos.z, + local_pos.vx, + local_pos.vy, + local_pos.vz); +} + +void +l_global_position_setpoint(const struct listener *l) +{ + struct vehicle_global_position_setpoint_s global_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); + + uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + + if (global_sp.altitude_is_relative) + coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + + if (gcs_link) + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + coordinate_frame, + global_sp.lat, + global_sp.lon, + global_sp.altitude, + global_sp.yaw); +} + +void +l_local_position_setpoint(const struct listener *l) +{ + struct vehicle_local_position_setpoint_s local_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); + + if (gcs_link) + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + MAV_FRAME_LOCAL_NED, + local_sp.x, + local_sp.y, + local_sp.z, + local_sp.yaw); +} + +void +l_attitude_setpoint(const struct listener *l) +{ + struct vehicle_attitude_setpoint_s att_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + att_sp.timestamp / 1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); +} + +void +l_vehicle_rates_setpoint(const struct listener *l) +{ + struct vehicle_rates_setpoint_s rates_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, + rates_sp.timestamp / 1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); +} + +void +l_actuator_outputs(const struct listener *l) +{ + struct actuator_outputs_s act_outputs; + + orb_id_t ids[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + /* copy actuator data into local buffer */ + orb_copy(ids[l->arg], *l->subp, &act_outputs); + + if (gcs_link) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + l->arg /* port number */, + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); + + /* only send in HIL mode */ + if (mavlink_hil_enabled && armed.armed) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); + + } else { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 500.0f, + (act_outputs.output[1] - 1500.0f) / 500.0f, + (act_outputs.output[2] - 1500.0f) / 500.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 500.0f, + (act_outputs.output[5] - 1500.0f) / 500.0f, + (act_outputs.output[6] - 1500.0f) / 500.0f, + (act_outputs.output[7] - 1500.0f) / 500.0f, + mavlink_mode, + 0); + } + } + } +} + +void +l_actuator_armed(const struct listener *l) +{ + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); +} + +void +l_manual_control_setpoint(const struct listener *l) +{ + struct manual_control_setpoint_s man_control; + + /* copy manual control data into local buffer */ + orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); + + if (gcs_link) + mavlink_msg_manual_control_send(MAVLINK_COMM_0, + mavlink_system.sysid, + man_control.roll * 1000, + man_control.pitch * 1000, + man_control.yaw * 1000, + man_control.throttle * 1000, + 0); +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + struct actuator_controls_effective_s actuators; + + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl0 ", + actuators.control_effective[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl1 ", + actuators.control_effective[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl2 ", + actuators.control_effective[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl3 ", + actuators.control_effective[3]); + } +} + +void +l_debug_key_value(const struct listener *l) +{ + struct debug_key_value_s debug; + + orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); + + /* Enforce null termination */ + debug.key[sizeof(debug.key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + debug.key, + debug.value); +} + +void +l_optical_flow(const struct listener *l) +{ + struct optical_flow_s flow; + + orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); + + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); +} + +void +l_home(const struct listener *l) +{ + struct home_position_s home; + + orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); +} + +static void * +uorb_receive_thread(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 1000; + + /* + * Initialise listener array. + * + * Might want to invoke each listener once to set initial state. + */ + struct pollfd fds[n_listeners]; + + for (unsigned i = 0; i < n_listeners; i++) { + fds[i].fd = *listeners[i].subp; + fds[i].events = POLLIN; + + /* Invoke callback to set initial state */ + //listeners[i].callback(&listener[i]); + } + + while (!thread_should_exit) { + + int poll_ret = poll(fds, n_listeners, timeout); + + /* handle the poll result */ + if (poll_ret == 0) { + mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + + } else if (poll_ret < 0) { + mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + + } else { + + for (unsigned i = 0; i < n_listeners; i++) { + if (fds[i].revents & POLLIN) + listeners[i].callback(&listeners[i]); + } + } + } + + return NULL; +} + +pthread_t +uorb_receive_start(void) +{ + /* --- SENSORS RAW VALUE --- */ + mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ + + /* --- ATTITUDE VALUE --- */ + mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ + + /* --- GPS VALUE --- */ + mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ + + /* --- HOME POSITION --- */ + mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ + + /* --- SYSTEM STATE --- */ + status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + + /* --- RC CHANNELS VALUE --- */ + rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(rc_sub, 100); /* 10Hz updates */ + + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + + /* --- GLOBAL POS VALUE --- */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ + + /* --- LOCAL POS VALUE --- */ + mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + + /* --- GLOBAL SETPOINT VALUE --- */ + mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ + + /* --- LOCAL SETPOINT VALUE --- */ + mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ + + /* --- ATTITUDE SETPOINT VALUE --- */ + mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + + /* --- RATES SETPOINT VALUE --- */ + mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + + /* --- ACTUATOR OUTPUTS --- */ + mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR ARMED VALUE --- */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR CONTROL VALUE --- */ + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ + + /* --- DEBUG VALUE OUTPUT --- */ + mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + + /* --- FLOW SENSOR --- */ + mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + + /* start the listener loop */ + pthread_attr_t uorb_attr; + pthread_attr_init(&uorb_attr); + + /* Set stack size, needs less than 2k */ + pthread_attr_setstacksize(&uorb_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + return thread; +} diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h new file mode 100644 index 000000000..d61cd43dc --- /dev/null +++ b/src/modules/mavlink/orb_topics.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 +#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; + int optical_flow; + int rates_setpoint_sub; + int home_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/util.h b/src/modules/mavlink/util.h new file mode 100644 index 000000000..a4ff06a88 --- /dev/null +++ b/src/modules/mavlink/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 + +/** MAVLink communications channel */ +extern uint8_t chan; + +/** Shutdown marker */ +extern volatile bool thread_should_exit; + +/** Waypoint storage */ +extern mavlink_wpm_storage *wpm; + +/** + * Translate the custom state into standard mavlink modes and state. + */ +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c new file mode 100644 index 000000000..a131b143b --- /dev/null +++ b/src/modules/mavlink/waypoints.c @@ -0,0 +1,1134 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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 waypoints.c + * MAVLink waypoint protocol implementation (BSD-relicensed). + */ + +#include +#include +#include +#include + +#include "missionlib.h" +#include "waypoints.h" +#include "util.h" + +#ifndef FM_PI +#define FM_PI 3.1415926535897932384626433832795f +#endif + +bool debug = false; +bool verbose = false; + + +#define MAVLINK_WPM_NO_PRINTF + +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + +void mavlink_wpm_init(mavlink_wpm_storage *state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = wpm->current_partner_sysid; + wpa.target_component = wpm->current_partner_compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + mavlink_missionlib_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + +#endif + mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, + cur->param2, cur->param3, cur->param4, cur->x, + cur->y, cur->z, cur->frame, cur->command); + + wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = wpm->current_partner_sysid; + wpc.target_component = wpm->current_partner_compid; + wpc.count = count; + + mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_message_t msg; + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); + wp->target_system = wpm->current_partner_sysid; + wp->target_component = wpm->current_partner_compid; + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->max_size) { + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = wpm->current_partner_sysid; + wpr.target_component = wpm->current_partner_compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm->size) +// { +// mavlink_mission_item_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm->size) +// { +// mavlink_mission_item_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// // if (verbose) // printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +/* + * Calculate distance in global frame. + * + * The distance calculation is based on the WGS84 geoid (GPS) + */ +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +{ + //TODO: implement for z once altidude contoller is implemented + + static uint16_t counter; + +// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + double current_x_rad = cur->x / 180.0 * M_PI; + double current_y_rad = cur->y / 180.0 * M_PI; + double x_rad = lat / 180.0 * M_PI; + double y_rad = lon / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; + + } else { + return -1.0f; + } + + counter++; + +} + +/* + * Calculate distance in local frame (NED) + */ +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + float dx = (cur->x - x); + float dy = (cur->y - y); + float dz = (cur->z - z); + + return sqrt(dx * dx + dy * dy + dz * dz); + + } else { + return -1.0f; + } +} + +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +{ + static uint16_t counter; + + // Do not flood the precious wireless link with debug data + // if (wpm->size > 0 && counter % 10 == 0) { + // printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); + // } + + + if (wpm->current_active_wp_id < wpm->size) { + + float orbit = wpm->waypoints[wpm->current_active_wp_id].param2; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; + float dist = -1.0f; + + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + + } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); + + } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + + } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { + /* Check if conditions of mission item are satisfied */ + // XXX TODO + } + + if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw + wpm->pos_reached = true; + + if (counter % 100 == 0) + printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); + } + +// else +// { +// if(counter % 100 == 0) +// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); +// } + } + + //check if the current waypoint was reached + if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->current_active_wp_id < wpm->size) { + mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); + + if (wpm->timestamp_firstinside_orbit == 0) { + // Announce that last waypoint was reached + printf("Reached waypoint %u for the first time \n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm->timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) { + printf("Reached waypoint %u long enough \n", cur_wp->seq); + + if (cur_wp->autocontinue) { + cur_wp->current = 0; + + if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { + /* the last waypoint was reached, if auto continue is + * activated restart the waypoint list from the beginning + */ + wpm->current_active_wp_id = 0; + + } else { + if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) + wpm->current_active_wp_id++; + } + + // Fly to next waypoint + wpm->timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->waypoints[wpm->current_active_wp_id].current = true; + wpm->pos_reached = false; + wpm->yaw_reached = false; + printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); + } + } + } + + } else { + wpm->timestamp_lastoutside_orbit = now; + } + + counter++; +} + + +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +{ + /* check for timed-out operations */ + if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); + +#endif + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_count = 0; + wpm->current_partner_sysid = 0; + wpm->current_partner_compid = 0; + wpm->current_wp_id = -1; + + if (wpm->size == 0) { + wpm->current_active_wp_id = -1; + } + } + + // Do NOT continously send the current WP, since we're not loosing messages + // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { + // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + // } + + check_waypoints_reached(now, global_position , local_position); + + return OK; +} + + +void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) +{ + uint64_t now = mavlink_missionlib_get_system_timestamp(); + + switch (msg->msgid) { +// case MAVLINK_MSG_ID_ATTITUDE: +// { +// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) +// { +// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) +// { +// mavlink_attitude_t att; +// mavlink_msg_attitude_decode(msg, &att); +// float yaw_tolerance = wpm->accept_range_yaw; +// //compare current yaw +// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) +// { +// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) +// wpm->yaw_reached = true; +// } +// else if(att.yaw - yaw_tolerance < 0.0f) +// { +// float lowerBound = 360.0f + att.yaw - yaw_tolerance; +// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) +// wpm->yaw_reached = true; +// } +// else +// { +// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; +// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) +// wpm->yaw_reached = true; +// } +// } +// } +// break; +// } +// +// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: +// { +// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) +// { +// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// +// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) +// { +// mavlink_local_position_ned_t pos; +// mavlink_msg_local_position_ned_decode(msg, &pos); +// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); +// +// wpm->pos_reached = false; +// +// // compare current position (given in message) with current waypoint +// float orbit = wp->param1; +// +// float dist; +//// if (wp->param2 == 0) +//// { +//// // FIXME segment distance +//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); +//// } +//// else +//// { +// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); +//// } +// +// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) +// { +// wpm->pos_reached = true; +// } +// } +// } +// break; +// } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm->size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_MISSION_ACK: { + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpm->current_wp_id == wpm->size - 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + +#endif + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_wp_id = 0; + } + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < wpm->size) { + // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); + wpm->current_active_wp_id = wpc.seq; + uint32_t i; + + for (i = 0; i < wpm->size; i++) { + if (i == wpm->current_active_wp_id) { + wpm->waypoints[i].current = true; + + } else { + wpm->waypoints[i].current = false; + } + } + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("NEW WP SET"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); + +#endif + wpm->yaw_reached = false; + wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->timestamp_firstinside_orbit = 0; + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpm->size > 0) { + //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + + } else { + // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + + wpm->current_count = wpm->size; + mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); + + } else { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); + } + } else { + // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST: { + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm->current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); + + } else { + // if (verbose) + { + if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); + +#endif + break; + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq != 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + +#endif + } + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + +#endif + + } else if (wpr.seq >= wpm->size) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); + +#endif + } + } + } + + } else { + //we we're target but already communicating with someone else + if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); + +#endif + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_COUNT: { + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { +// printf("wpc count in: %d\n",wpc.count); +// printf("Comp id: %d\n",msg->compid); +// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); + + if (wpc.count > 0) { + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + +#endif + } + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + wpm->current_count = wpc.count; + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); + +#endif + wpm->rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (wpc.count == 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("COUNT 0"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + +#endif + wpm->rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + break; + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CMD"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + +#endif + } + + } else { + if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); + +#endif + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); + +#endif + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); + +#endif + } + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + } + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + mavlink_missionlib_send_gcs_string("GOT WP"); +// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); +// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); + +// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) + if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { + + wpm->timestamp_lastaction = now; + +// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); + +// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { + //mavlink_missionlib_send_gcs_string("DEBUG 2"); + +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); +// + wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); +// printf("WP seq: %d\n",wp.seq); + wpm->current_wp_id = wp.seq + 1; + + // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); +// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + +// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); + if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + mavlink_missionlib_send_gcs_string("GOT ALL WPS"); + // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + + if (wpm->current_active_wp_id > wpm->rcv_size - 1) { + wpm->current_active_wp_id = wpm->rcv_size - 1; + } + + // switch the waypoints list + // FIXME CHECK!!! + uint32_t i; + + for (i = 0; i < wpm->current_count; ++i) { + wpm->waypoints[i] = wpm->rcv_waypoints[i]; + } + + wpm->size = wpm->current_count; + + //get the new current waypoint + + for (i = 0; i < wpm->size; i++) { + if (wpm->waypoints[i].current == 1) { + wpm->current_active_wp_id = i; + //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); + wpm->yaw_reached = false; + wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm->size) { + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + wpm->timestamp_firstinside_orbit = 0; + } + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + + } else { + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + } + + } else { + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + + // if (verbose) + { + if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); + break; + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + if (!(wp.seq == 0)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + if (!(wp.seq == wpm->current_wp_id)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (!(wp.seq < wpm->current_count)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } + } + } else { + //we we're target but already communicating with someone else + if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); + } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + wpm->timestamp_lastaction = now; + + // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm->size = 0; + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); + } + + break; + } + + default: { + // if (debug) // printf("Waypoint: received message of unknown type"); + break; + } + } + + check_waypoints_reached(now, global_pos, local_pos); +} diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h new file mode 100644 index 000000000..c32ab32e5 --- /dev/null +++ b/src/modules/mavlink/waypoints.h @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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 waypoints.h + * MAVLink waypoint protocol definition (BSD-relicensed). + */ + +#ifndef WAYPOINTS_H_ +#define WAYPOINTS_H_ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +#include + +#ifndef MAVLINK_SEND_UART_BYTES +#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) +#endif +extern mavlink_system_t mavlink_system; +#include +#include +#include +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 15 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +void mavlink_wpm_init(mavlink_wpm_storage *state); +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, + struct vehicle_local_position_s *local_pos); +void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , + struct vehicle_local_position_s *local_pos); + +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); + +#endif /* WAYPOINTS_H_ */ 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 63136e354330bcf8efe2757187515eeaa8bc3bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:11:16 +0200 Subject: Resurrected C++ change commit, now back up to same state as master --- src/modules/attitude_estimator_ekf/Makefile | 57 +++ .../attitude_estimator_ekf_main.c | 464 -------------------- .../attitude_estimator_ekf_main.cpp | 472 +++++++++++++++++++++ src/modules/attitude_estimator_ekf/module.mk | 63 ++- 4 files changed, 579 insertions(+), 477 deletions(-) create mode 100755 src/modules/attitude_estimator_ekf/Makefile delete mode 100755 src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c create mode 100755 src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp diff --git a/src/modules/attitude_estimator_ekf/Makefile b/src/modules/attitude_estimator_ekf/Makefile new file mode 100755 index 000000000..46a54c660 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/Makefile @@ -0,0 +1,57 @@ +############################################################################ +# +# 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. +# +############################################################################ + +APPNAME = attitude_estimator_ekf +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +CXXSRCS = attitude_estimator_ekf_main.cpp + +CSRCS = attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c + + +# XXX this is *horribly* broken +INCLUDES += $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c deleted file mode 100755 index 9fc4dfc83..000000000 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ /dev/null @@ -1,464 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * 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 attitude_estimator_ekf_main.c - * - * Extended Kalman Filter for Attitude Estimation. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" -#include "attitude_estimator_ekf_params.h" - -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ - -/** - * Mainloop of attitude_estimator_ekf. - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The attitude_estimator_ekf app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int attitude_estimator_ekf_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("attitude_estimator_ekf already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 12400, - attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); - - } else { - printf("\tattitude_estimator_ekf app not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/* - * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - -/* - * EKF Attitude Estimator main function. - * - * Estimates the attitude recursively once started. - * - * @param argc number of commandline arguments (plus command name) - * @param argv strings containing the arguments - */ -int attitude_estimator_ekf_thread_main(int argc, char *argv[]) -{ - -const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - - float dt = 0.005f; -/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ - float x_aposteriori_k[12]; /**< states */ - float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ - - float x_aposteriori[12]; - float P_aposteriori[144]; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - - int overloadcounter = 19; - - /* Initialize filter */ - attitudeKalmanfilter_initialize(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - - - int loopcounter = 0; - int printcounter = 0; - - thread_running = true; - - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - - /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; - uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - - struct attitude_estimator_ekf_params ekf_params; - - struct attitude_estimator_ekf_param_handles ekf_param_handles; - - /* initialize parameter handles */ - parameters_init(&ekf_param_handles); - - uint64_t start_time = hrt_absolute_time(); - bool initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; - - /* register the perf counter */ - perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); - - /* Main loop*/ - while (!thread_should_exit) { - - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; - int ret = poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); - - if (!state.flag_hil_enabled) { - fprintf(stderr, - "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); - } - - } else { - - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&ekf_param_handles, &ekf_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } - - } else { - - perf_begin(ekf_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - - static bool const_initialized = false; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { - dt = 0.005f; - parameters_update(&ekf_param_handles, &ekf_params); - - x_aposteriori_k[0] = z_k[0]; - x_aposteriori_k[1] = z_k[1]; - x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = 0.0f; - x_aposteriori_k[4] = 0.0f; - x_aposteriori_k[5] = 0.0f; - x_aposteriori_k[6] = z_k[3]; - x_aposteriori_k[7] = z_k[4]; - x_aposteriori_k[8] = z_k[5]; - x_aposteriori_k[9] = z_k[6]; - x_aposteriori_k[10] = z_k[7]; - x_aposteriori_k[11] = z_k[8]; - - const_initialized = true; - } - - /* do not execute the filter if not initialized */ - if (!const_initialized) { - continue; - } - - uint64_t timing_start = hrt_absolute_time(); - - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); - - /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); - - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - continue; - } - - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; - - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; - att.rollacc = x_aposteriori[3]; - att.pitchacc = x_aposteriori[4]; - att.yawacc = x_aposteriori[5]; - - //att.yawspeed =z_k[2] ; - /* copy offsets */ - memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); - att.R_valid = true; - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - - } else { - warnx("NaN in roll/pitch/yaw estimate!"); - } - - perf_end(ekf_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp new file mode 100755 index 000000000..1a50dde0f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -0,0 +1,472 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * 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 attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif + +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of attitude_estimator_ekf. + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The attitude_estimator_ekf app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int attitude_estimator_ekf_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("attitude_estimator_ekf already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12400, + attitude_estimator_ekf_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tattitude_estimator_ekf app is running\n"); + + } else { + printf("\tattitude_estimator_ekf app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]) +{ + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 4); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise attitude */ + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + /* advertise debug value */ + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* initialize parameter handles */ + parameters_init(&ekf_param_handles); + + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + + /* register the perf counter */ + perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } + + } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; + + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + + } else { + + perf_begin(ekf_loop_perf); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05f && dt > 0.005f) { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] - ekf_params.yaw_off; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + + //att.yawspeed =z_k[2] ; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + } + + perf_end(ekf_loop_perf); + } + } + } + + loopcounter++; + } + + thread_running = false; + + return 0; +} diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 4033617c4..3034018aa 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -1,16 +1,53 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Attitude estimator (Extended Kalman Filter) +# MODULE_NAME = attitude_estimator_ekf -SRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/attitudeKalmanfilter.c \ - codegen/cross.c \ - codegen/eye.c \ - codegen/mrdivide.c \ - codegen/norm.c \ - codegen/rdivide.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c +CXXSRCS = attitude_estimator_ekf_main.cpp + +SRCS = attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c -- cgit v1.2.3 From 0c4b8a54c7779d198e016470889456b3458e2303 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:13:03 +0200 Subject: Deleted old-style EEPROM driver, new one is already functional --- apps/systemcmds/eeprom/24xxxx_mtd.c | 571 ------------------------------------ apps/systemcmds/eeprom/Makefile | 45 --- apps/systemcmds/eeprom/eeprom.c | 265 ----------------- 3 files changed, 881 deletions(-) delete mode 100644 apps/systemcmds/eeprom/24xxxx_mtd.c delete mode 100644 apps/systemcmds/eeprom/Makefile delete mode 100644 apps/systemcmds/eeprom/eeprom.c diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c deleted file mode 100644 index e34be44e3..000000000 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ /dev/null @@ -1,571 +0,0 @@ -/************************************************************************************ - * Driver for 24xxxx-style I2C EEPROMs. - * - * Adapted from: - * - * drivers/mtd/at24xx.c - * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) - * - * Copyright (C) 2011 Li Zhuoyi. All rights reserved. - * Author: Li Zhuoyi - * History: 0.1 2011-08-20 initial version - * - * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn - * - * Derived from drivers/mtd/m25px.c - * - * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "systemlib/perf_counter.h" - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ - -/* - * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be - * omitted in order to avoid building the AT24XX driver as well. - */ - -/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ - -#ifndef CONFIG_AT24XX_SIZE -# warning "Assuming AT24 size 64" -# define CONFIG_AT24XX_SIZE 64 -#endif -#ifndef CONFIG_AT24XX_ADDR -# warning "Assuming AT24 address of 0x50" -# define CONFIG_AT24XX_ADDR 0x50 -#endif - -/* Get the part configuration based on the size configuration */ - -#if CONFIG_AT24XX_SIZE == 32 -# define AT24XX_NPAGES 128 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 48 -# define AT24XX_NPAGES 192 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 64 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 128 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 64 -#elif CONFIG_AT24XX_SIZE == 256 -# define AT24XX_NPAGES 512 -# define AT24XX_PAGESIZE 64 -#endif - -/* For applications where a file system is used on the AT24, the tiny page sizes - * will result in very inefficient FLASH usage. In such cases, it is better if - * blocks are comprised of "clusters" of pages so that the file system block - * size is, say, 256 or 512 bytes. In any event, the block size *must* be an - * even multiple of the pages. - */ - -#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE -# warning "Assuming driver block size is the same as the FLASH page size" -# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE -#endif - -/* The AT24 does not respond on the bus during write cycles, so we depend on a long - * timeout to detect problems. The max program time is typically ~5ms. - */ -#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS -# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 -#endif - -/************************************************************************************ - * Private Types - ************************************************************************************/ - -/* This type represents the state of the MTD device. The struct mtd_dev_s - * must appear at the beginning of the definition so that you can freely - * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. - */ - -struct at24c_dev_s { - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; -}; - -/************************************************************************************ - * Private Function Prototypes - ************************************************************************************/ - -/* MTD driver methods */ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); - -void at24c_test(void); - -/************************************************************************************ - * Private Data - ************************************************************************************/ - -/* At present, only a single AT24 part is supported. In this case, a statically - * allocated state structure may be used. - */ - -static struct at24c_dev_s g_at24c; - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -static int at24c_eraseall(FAR struct at24c_dev_s *priv) -{ - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2], 0xff, priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; -} - -/************************************************************************************ - * Name: at24c_erase - ************************************************************************************/ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) -{ - /* EEprom need not erase */ - - return (int)nblocks; -} - -/************************************************************************************ - * Name: at24c_test - ************************************************************************************/ - -void at24c_test(void) -{ - uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; - unsigned count = 0; - unsigned errors = 0; - - for (count = 0; count < 10000; count++) { - ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); - if (result == ERROR) { - if (errors++ > 2) { - vdbg("too many errors\n"); - return; - } - } else if (result != 1) { - vdbg("unexpected %u\n", result); - } - if ((count % 100) == 0) - vdbg("test %u errors %u\n", count, errors); - } -} - -/************************************************************************************ - * Name: at24c_bread - ************************************************************************************/ - -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - - if (--tries == 0) { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_bwrite - * - * Operates on MTD block's and translates to FLASH pages - * - ************************************************************************************/ - -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE + 2]; - int ret; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); - - for (;;) { - - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); - - if (ret >= 0) - break; - - fvdbg("write stall"); - usleep(1000); - - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) { - perf_count(priv->perf_write_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_ioctl - ************************************************************************************/ - -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) { - case MTDIOC_GEOMETRY: { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - - if (geo) { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; -#else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; -#endif - ret = OK; - - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; - - case MTDIOC_BULKERASE: - ret = at24c_eraseall(priv); - break; - - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } - - return ret; -} - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: at24c_initialize - * - * Description: - * Create an initialize MTD device instance. MTD devices are not registered - * in the file system, but are created as instances that can be bound to - * other functions (such as a block or character driver front end). - * - ************************************************************************************/ - -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - - if (priv) { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* attempt to read to validate device is present */ - unsigned char buf[5]; - uint8_t addrbuf[2] = {0, 0}; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addrbuf[0], - .length = sizeof(addrbuf), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - perf_begin(priv->perf_reads); - int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret < 0) { - return NULL; - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; -} - -/* - * XXX: debug hackery - */ -int at24c_nuke(void) -{ - return at24c_eraseall(&g_at24c); -} diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile deleted file mode 100644 index 58c8cb5ec..000000000 --- a/apps/systemcmds/eeprom/Makefile +++ /dev/null @@ -1,45 +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 = eeprom -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 -MAXOPTIMIZATION = -Os - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 49da51358..000000000 --- a/apps/systemcmds/eeprom/eeprom.c +++ /dev/null @@ -1,265 +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 eeprom.c - * - * EEPROM service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -#ifndef PX4_I2C_BUS_ONBOARD -# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM -#endif - -__EXPORT int eeprom_main(int argc, char *argv[]); - -static void eeprom_attach(void); -static void eeprom_start(void); -static void eeprom_erase(void); -static void eeprom_ioctl(unsigned operation); -static void eeprom_save(const char *name); -static void eeprom_load(const char *name); -static void eeprom_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *eeprom_mtd; - -int eeprom_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - eeprom_start(); - - if (!strcmp(argv[1], "save_param")) - eeprom_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - eeprom_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - eeprom_erase(); - - if (!strcmp(argv[1], "test")) - eeprom_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - eeprom_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - eeprom_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); -} - - -static void -eeprom_attach(void) -{ - /* find the right I2C */ - struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - /* this resets the I2C bus, set correct bus speed again */ - I2C_SETFREQUENCY(i2c, 400000); - - if (i2c == NULL) - errx(1, "failed to locate I2C bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - eeprom_mtd = at24c_initialize(i2c); - if (eeprom_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (eeprom_mtd == NULL) - errx(1, "failed to initialize EEPROM driver"); - - attached = true; -} - -static void -eeprom_start(void) -{ - int ret; - - if (started) - errx(1, "EEPROM already mounted"); - - if (!attached) - eeprom_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(eeprom_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - - /* mount the EEPROM */ - ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - - started = true; - warnx("mounted EEPROM at /eeprom"); - exit(0); -} - -extern int at24c_nuke(void); - -static void -eeprom_erase(void) -{ - if (!attached) - eeprom_attach(); - - if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -eeprom_ioctl(unsigned operation) -{ - int fd; - - fd = open("/eeprom/.", 0); - - if (fd < 0) - err(1, "open /eeprom"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -eeprom_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -eeprom_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -extern void at24c_test(void); - -static void -eeprom_test(void) -{ - at24c_test(); - exit(0); -} -- cgit v1.2.3 From 680b48e048973887a65e908d475f2c0f7afe5fd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 00:13:35 +0200 Subject: Deleted old cruft --- apps/systemcmds/delay_test/Makefile | 44 --------- apps/systemcmds/delay_test/delay_test.c | 160 -------------------------------- 2 files changed, 204 deletions(-) delete mode 100644 apps/systemcmds/delay_test/Makefile delete mode 100644 apps/systemcmds/delay_test/delay_test.c diff --git a/apps/systemcmds/delay_test/Makefile b/apps/systemcmds/delay_test/Makefile deleted file mode 100644 index e54cf2f4e..000000000 --- a/apps/systemcmds/delay_test/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. -# -############################################################################ - -# -# Delay test -# - -APPNAME = delay_test -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk - -MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/delay_test/delay_test.c b/apps/systemcmds/delay_test/delay_test.c deleted file mode 100644 index 51cce38fc..000000000 --- a/apps/systemcmds/delay_test/delay_test.c +++ /dev/null @@ -1,160 +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 delay_test.c - * - * Simple but effective delay test using leds and a scope to measure - * communication delays end-to-end accurately. - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include - -__EXPORT int delay_test_main(int argc, char *argv[]); -static int led_on(int leds, int led); -static int led_off(int leds, int led); - -int delay_test_main(int argc, char *argv[]) -{ - bool vicon_msg = false; - bool command_msg = false; - - if (argc > 1 && !strcmp(argv[1], "--help")) { - warnx("usage: delay_test [vicon] [command]\n"); - exit(1); - } - - if (argc > 1 && !strcmp(argv[1], "vicon")) { - vicon_msg = true; - } - - if (argc > 1 && !strcmp(argv[1], "command")) { - command_msg = true; - } - - int buzzer = open("/dev/tone_alarm", O_WRONLY); - int leds = open(LED_DEVICE_PATH, 0); - - /* prepare use of amber led */ - led_off(leds, LED_AMBER); - - int topic; - - /* subscribe to topic */ - if (vicon_msg) { - topic = orb_subscribe(ORB_ID(vehicle_vicon_position)); - } else if (command_msg) { - topic = orb_subscribe(ORB_ID(vehicle_command)); - } else { - errx(1, "No topic selected for delay test, use --help for a list of topics."); - } - - const int testcount = 20; - - warnx("running %d iterations..\n", testcount); - - struct pollfd fds[1]; - fds[0].fd = topic; - fds[0].events = POLLIN; - - /* display and sound error */ - for (int i = 0; i < testcount; i++) - { - /* wait for topic */ - int ret = poll(&fds[0], 1, 2000); - - /* this would be bad... */ - if (ret < 0) { - warnx("poll error %d", errno); - usleep(1000000); - continue; - } - - /* do we have a topic update? */ - if (fds[0].revents & POLLIN) { - - /* copy object to disable poll ready state */ - if (vicon_msg) { - struct vehicle_vicon_position_s vicon_position; - orb_copy(ORB_ID(vehicle_vicon_position), topic, &vicon_position); - } else if (command_msg) { - struct vehicle_command_s vehicle_command; - orb_copy(ORB_ID(vehicle_command), topic, &vehicle_command); - } - - led_on(leds, LED_AMBER); - ioctl(buzzer, TONE_SET_ALARM, 4); - /* keep led on for 50 ms to make it barely visible */ - usleep(50000); - led_off(leds, LED_AMBER); - } - } - - /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); - - /* switch on leds */ - led_on(leds, LED_BLUE); - led_on(leds, LED_AMBER); - - exit(0); -} - -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 -- cgit v1.2.3 From 686139c7d8229365fd3b8f4bd6cdaab0d2f06b8b Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 23:09:11 -0700 Subject: HACK: don't call the card-changed callback with interrupts disabled, as it means that timeouts don't work. --- nuttx/arch/arm/src/stm32/stm32_sdio.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_sdio.c b/nuttx/arch/arm/src/stm32/stm32_sdio.c index a8bcae307..dad94e8a1 100644 --- a/nuttx/arch/arm/src/stm32/stm32_sdio.c +++ b/nuttx/arch/arm/src/stm32/stm32_sdio.c @@ -2797,13 +2797,14 @@ void sdio_mediachange(FAR struct sdio_dev_s *dev, bool cardinslot) } fvdbg("cdstatus OLD: %02x NEW: %02x\n", cdstatus, priv->cdstatus); + irqrestore(flags); + /* Perform any requested callback if the status has changed */ if (cdstatus != priv->cdstatus) { stm32_callback(priv); } - irqrestore(flags); } /**************************************************************************** -- cgit v1.2.3 From 9d4d1ace437f94bfc517b7ff9036657fd5b2c354 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 23:09:38 -0700 Subject: Pick up the MAVlink headers from the right place --- src/modules/attitude_estimator_ekf/module.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 5ce545112..15d17e30d 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -14,3 +14,4 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/rtGetInf.c \ codegen/rtGetNaN.c +INCLUDE_DIRS += $(PX4_BASE)/mavlink/include/mavlink -- cgit v1.2.3 From 2289c0bb216027419a9ba5e52103a1310ff03292 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 new build system --- 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 | 24 +- makefiles/config_px4fmuv2_default.mk | 23 +- 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/.context | 0 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/.context | 0 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/.context | 0 src/systemcmds/reboot/module.mk | 41 +++ src/systemcmds/reboot/reboot.c | 53 +++ src/systemcmds/top/.context | 0 src/systemcmds/top/module.mk | 44 +++ src/systemcmds/top/top.c | 253 +++++++++++++ 58 files changed, 2521 insertions(+), 3431 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/.context 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/.context 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/.context create mode 100644 src/systemcmds/reboot/module.mk create mode 100644 src/systemcmds/reboot/reboot.c create mode 100644 src/systemcmds/top/.context create mode 100644 src/systemcmds/top/module.mk create mode 100644 src/systemcmds/top/top.c 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 ada6b7ab7..037ebe2b9 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -15,7 +15,21 @@ MODULES += drivers/boards/px4fmu MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/ardrone_interface + +# +# 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 # # General system control @@ -44,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 ) \ @@ -58,23 +69,16 @@ 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, tests, , 12000, tests_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/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 9aa4ec3da..5b3786ce2 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,7 +15,20 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/px4fmu MODULES += drivers/rgbled + +# +# System commands +# MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/reboot +MODULES += systemcmds/top # # General system control @@ -44,12 +57,8 @@ 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, boardinfo, , 2048, boardinfo_main ) \ - $(call _B, commander, SCHED_PRIORITY_MAX-30, 2048, commander_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 ) \ @@ -57,19 +66,13 @@ 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, 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, perf, , 2048, perf_main ) \ $(call _B, position_estimator, , 4096, position_estimator_main ) \ - $(call _B, preflight_check, , 2048, preflight_check_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, tests, , 12000, tests_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/.context b/src/systemcmds/boardinfo/.context new file mode 100644 index 000000000..e69de29bb 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/.context b/src/systemcmds/perf/.context new file mode 100644 index 000000000..e69de29bb 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/.context b/src/systemcmds/reboot/.context new file mode 100644 index 000000000..e69de29bb 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/.context b/src/systemcmds/top/.context new file mode 100644 index 000000000..e69de29bb 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 From a85eb8cdc12305b17a6d7de5f00327f2a8e096ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 13:40:11 +0200 Subject: Updated config file to reflect current app state --- makefiles/config_px4fmuv2_default.mk | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 5b3786ce2..d786f9dc0 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,11 +10,12 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # +MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -MODULES += drivers/px4fmu -MODULES += drivers/rgbled +MODULES += modules/sensors # # System commands @@ -29,6 +30,7 @@ MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top +MODULES += systemcmds/tests # # General system control @@ -41,6 +43,12 @@ MODULES += modules/mavlink_onboard # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/position_estimator_mc + +# +# Logging +# +MODULES += modules/sdlog # # Transitional support - add commands from the NuttX export archive. @@ -69,10 +77,7 @@ BUILTIN_COMMANDS := \ $(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, position_estimator, , 4096, position_estimator_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, tests, , 12000, tests_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ $(call _B, uorb, , 4096, uorb_main ) -- cgit v1.2.3 From f03ba89557417f9805ba95c13936d358a26af7d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 18:27:17 +0200 Subject: Config adjustments for v2 --- makefiles/config_px4fmuv2_default.mk | 19 +++++++++++++++++++ nuttx/configs/px4fmuv2/include/board.h | 1 + 2 files changed, 20 insertions(+) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index d786f9dc0..68e5fa916 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,6 +15,14 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 +MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott_telemetry +MODULES += drivers/blinkm MODULES += modules/sensors # @@ -44,6 +52,17 @@ MODULES += modules/mavlink_onboard # MODULES += modules/attitude_estimator_ekf MODULES += modules/position_estimator_mc +MODULES += modules/position_estimator +MODULES += modules/att_pos_estimator_ekf + +# +# Vehicle Control +# +MODULES += modules/fixedwing_backside +MODULES += modules/fixedwing_att_control +MODULES += modules/fixedwing_pos_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control # # Logging diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h index be4cdcdfd..0055d65e1 100755 --- a/nuttx/configs/px4fmuv2/include/board.h +++ b/nuttx/configs/px4fmuv2/include/board.h @@ -295,6 +295,7 @@ * Note that these are unshifted addresses. */ #define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e /* * SPI -- cgit v1.2.3 From 86cf4d075dae90db4a681261f4e378d41d8bcf49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 19:50:25 +0200 Subject: Makefile cleanup --- makefiles/config_px4fmuv2_default.mk | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 68e5fa916..47cd09748 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,9 +15,9 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -MODULES += drivers/mpu6000 +#MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 +#MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil @@ -84,18 +84,7 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, adc, , 2048, adc_main ) \ - $(call _B, blinkm, , 2048, blinkm_main ) \ - $(call _B, control_demo, , 2048, control_demo_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 ) \ - $(call _B, hil, , 2048, hil_main ) \ - $(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, 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, position_estimator, , 4096, position_estimator_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ $(call _B, serdis, , 2048, serdis_main ) \ $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ -- cgit v1.2.3 From 13d58afd0a57698710c18fd1ffc6192d6aabe07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 11:03:25 +0200 Subject: Updated FMUv2 config --- makefiles/config_px4fmuv2_default.mk | 36 +++++++++++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 5 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 47cd09748..ab1f19314 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -10,6 +10,10 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled @@ -69,6 +73,32 @@ MODULES += modules/multirotor_pos_control # MODULES += modules/sdlog +# +# Libraries +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/mathlib +MODULES += modules/mathlib/CMSIS +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + # # Transitional support - add commands from the NuttX export archive. # @@ -83,9 +113,5 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ - $(call _B, adc, , 2048, adc_main ) \ - $(call _B, math_demo, , 8192, math_demo_main ) \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, tone_alarm, , 2048, tone_alarm_main ) \ - $(call _B, uorb, , 4096, uorb_main ) + $(call _B, serdis, , 2048, serdis_main ) -- cgit v1.2.3 From 1df5e98aa507c7a89f1491254d7f34f94c04ede6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 14:50:05 +0200 Subject: XXX: WIP: Disabled mixer on IOv2 due to CXX compile issue --- Images/px4iov2.prototype | 12 + apps/drivers/boards/px4iov2/Makefile | 41 -- makefiles/board_px4iov2.mk | 10 + makefiles/config_px4iov2_default.mk | 10 + nuttx/configs/px4fmuv2/nsh/appconfig | 86 ---- nuttx/configs/px4iov2/include/board.h | 11 + nuttx/configs/px4iov2/include/drv_i2c_device.h | 42 -- nuttx/configs/px4iov2/io/appconfig | 8 - nuttx/configs/px4iov2/io/defconfig | 2 +- nuttx/configs/px4iov2/src/drv_i2c_device.c | 618 ------------------------- src/drivers/boards/px4iov2/module.mk | 6 + src/drivers/boards/px4iov2/px4iov2_init.c | 171 +++++++ src/drivers/boards/px4iov2/px4iov2_internal.h | 112 +++++ src/drivers/boards/px4iov2/px4iov2_pwm_servo.c | 123 +++++ 14 files changed, 456 insertions(+), 796 deletions(-) create mode 100644 Images/px4iov2.prototype delete mode 100644 apps/drivers/boards/px4iov2/Makefile create mode 100644 makefiles/board_px4iov2.mk create mode 100644 makefiles/config_px4iov2_default.mk delete mode 100644 nuttx/configs/px4iov2/include/drv_i2c_device.h delete mode 100644 nuttx/configs/px4iov2/src/drv_i2c_device.c create mode 100644 src/drivers/boards/px4iov2/module.mk create mode 100644 src/drivers/boards/px4iov2/px4iov2_init.c create mode 100755 src/drivers/boards/px4iov2/px4iov2_internal.h create mode 100644 src/drivers/boards/px4iov2/px4iov2_pwm_servo.c diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype new file mode 100644 index 000000000..af87924e9 --- /dev/null +++ b/Images/px4iov2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/apps/drivers/boards/px4iov2/Makefile b/apps/drivers/boards/px4iov2/Makefile deleted file mode 100644 index 85806fe6f..000000000 --- a/apps/drivers/boards/px4iov2/Makefile +++ /dev/null @@ -1,41 +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. -# -############################################################################ - -# -# Board-specific startup code for the PX4IO -# - -INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common -LIBNAME = brd_px4io - -include $(APPDIR)/mk/app.mk diff --git a/makefiles/board_px4iov2.mk b/makefiles/board_px4iov2.mk new file mode 100644 index 000000000..ee6b6125e --- /dev/null +++ b/makefiles/board_px4iov2.mk @@ -0,0 +1,10 @@ +# +# Board-specific definitions for the PX4IOv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM3 + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_px4iov2_default.mk b/makefiles/config_px4iov2_default.mk new file mode 100644 index 000000000..f9f35d629 --- /dev/null +++ b/makefiles/config_px4iov2_default.mk @@ -0,0 +1,10 @@ +# +# Makefile for the px4iov2_default configuration +# + +# +# Board support modules +# +MODULES += drivers/stm32 +MODULES += drivers/boards/px4iov2 +MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig index d945f3d88..0e18aa8ef 100644 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ b/nuttx/configs/px4fmuv2/nsh/appconfig @@ -41,92 +41,6 @@ CONFIGURED_APPS += examples/nsh CONFIGURED_APPS += nshlib CONFIGURED_APPS += system/readline -# System library - utility functions -CONFIGURED_APPS += systemlib -CONFIGURED_APPS += systemlib/mixer - -# Math library -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += mathlib -CONFIGURED_APPS += mathlib/CMSIS -CONFIGURED_APPS += examples/math_demo -endif - -# Control library -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += controllib -CONFIGURED_APPS += examples/control_demo -CONFIGURED_APPS += examples/kalman_demo -endif - -# System utility commands -CONFIGURED_APPS += systemcmds/reboot -CONFIGURED_APPS += systemcmds/perf -CONFIGURED_APPS += systemcmds/top -CONFIGURED_APPS += systemcmds/boardinfo -CONFIGURED_APPS += systemcmds/mixer -CONFIGURED_APPS += systemcmds/param -CONFIGURED_APPS += systemcmds/pwm -CONFIGURED_APPS += systemcmds/bl_update -CONFIGURED_APPS += systemcmds/preflight_check -CONFIGURED_APPS += systemcmds/delay_test - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -# CONFIGURED_APPS += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/deamon -# CONFIGURED_APPS += examples/px4_deamon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -# CONFIGURED_APPS += examples/px4_mavlink_debug - -# Shared object broker; required by many parts of the system. -CONFIGURED_APPS += uORB - -CONFIGURED_APPS += mavlink -CONFIGURED_APPS += mavlink_onboard -CONFIGURED_APPS += commander -CONFIGURED_APPS += sdlog -CONFIGURED_APPS += sensors - -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += multirotor_att_control -CONFIGURED_APPS += multirotor_pos_control -CONFIGURED_APPS += fixedwing_att_control -CONFIGURED_APPS += fixedwing_pos_control -CONFIGURED_APPS += position_estimator -CONFIGURED_APPS += attitude_estimator_ekf -CONFIGURED_APPS += hott_telemetry -endif - -# Hacking tools -# XXX needs updating for new i2c config -#CONFIGURED_APPS += systemcmds/i2c - -# Communication and Drivers -CONFIGURED_APPS += drivers/boards/px4fmuv2 -CONFIGURED_APPS += drivers/device -# XXX needs conversion to SPI -#CONFIGURED_APPS += drivers/ms5611 -# XXX needs conversion to serial -#CONFIGURED_APPS += drivers/px4io -CONFIGURED_APPS += drivers/stm32 -#CONFIGURED_APPS += drivers/led -CONFIGURED_APPS += drivers/blinkm -CONFIGURED_APPS += drivers/stm32/tone_alarm -CONFIGURED_APPS += drivers/stm32/adc -#CONFIGURED_APPS += drivers/px4fmu -CONFIGURED_APPS += drivers/hil -CONFIGURED_APPS += drivers/gps -CONFIGURED_APPS += drivers/mb12xx - -# Testing stuff -CONFIGURED_APPS += px4/sensors_bringup -CONFIGURED_APPS += px4/tests - ifeq ($(CONFIG_CAN),y) #CONFIGURED_APPS += examples/can endif diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx/configs/px4iov2/include/board.h index b8dc71144..21aacda87 100755 --- a/nuttx/configs/px4iov2/include/board.h +++ b/nuttx/configs/px4iov2/include/board.h @@ -100,12 +100,19 @@ * Some of the USART pins are not available; override the GPIO * definitions with an invalid pin configuration. */ +#undef GPIO_USART2_CTS #define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS #define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK #define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_TX #define GPIO_USART3_TX 0xffffffff +#undef GPIO_USART3_CK #define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS #define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS #define GPIO_USART3_RTS 0xffffffff /* @@ -168,6 +175,10 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); + +#if defined(__cplusplus) +} +#endif #endif /* __ASSEMBLY__ */ #endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4iov2/include/drv_i2c_device.h b/nuttx/configs/px4iov2/include/drv_i2c_device.h deleted file mode 100644 index 02582bc09..000000000 --- a/nuttx/configs/px4iov2/include/drv_i2c_device.h +++ /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. - * - ****************************************************************************/ - - /** - * @file A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); -extern bool i2c_fsm(void); diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx/configs/px4iov2/io/appconfig index 628607a51..48a41bcdb 100644 --- a/nuttx/configs/px4iov2/io/appconfig +++ b/nuttx/configs/px4iov2/io/appconfig @@ -30,11 +30,3 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -CONFIGURED_APPS += drivers/boards/px4io -CONFIGURED_APPS += drivers/stm32 - -CONFIGURED_APPS += px4io - -# Mixer library from systemlib -CONFIGURED_APPS += systemlib/mixer diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig index bb937cf4e..c73f0df89 100755 --- a/nuttx/configs/px4iov2/io/defconfig +++ b/nuttx/configs/px4iov2/io/defconfig @@ -127,7 +127,7 @@ CONFIG_STM32_WWDG=n CONFIG_STM32_SPI2=n CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C1=n CONFIG_STM32_I2C2=n CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n diff --git a/nuttx/configs/px4iov2/src/drv_i2c_device.c b/nuttx/configs/px4iov2/src/drv_i2c_device.c deleted file mode 100644 index 1f5931ae5..000000000 --- a/nuttx/configs/px4iov2/src/drv_i2c_device.c +++ /dev/null @@ -1,618 +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 A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -#include - -#include "stm32_i2c.h" - -#include - -/* - * I2C register definitions. - */ -#define I2C_BASE STM32_I2C1_BASE - -#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) - -#define rCR1 REG(STM32_I2C_CR1_OFFSET) -#define rCR2 REG(STM32_I2C_CR2_OFFSET) -#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) -#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) -#define rDR REG(STM32_I2C_DR_OFFSET) -#define rSR1 REG(STM32_I2C_SR1_OFFSET) -#define rSR2 REG(STM32_I2C_SR2_OFFSET) -#define rCCR REG(STM32_I2C_CCR_OFFSET) -#define rTRISE REG(STM32_I2C_TRISE_OFFSET) - -/* - * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib - */ -#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ -#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ -#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ -#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ -#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ - -/** - * States implemented by the I2C FSM. - */ -enum fsm_state { - BAD_PHASE, // must be zero, default exit on a bad state transition - - WAIT_FOR_MASTER, - - /* write from master */ - WAIT_FOR_COMMAND, - RECEIVE_COMMAND, - RECEIVE_DATA, - HANDLE_COMMAND, - - /* read from master */ - WAIT_TO_SEND, - SEND_STATUS, - SEND_DATA, - - NUM_STATES -}; - -/** - * Events recognised by the I2C FSM. - */ -enum fsm_event { - /* automatic transition */ - AUTO, - - /* write from master */ - ADDRESSED_WRITE, - BYTE_RECEIVED, - STOP_RECEIVED, - - /* read from master */ - ADDRESSED_READ, - BYTE_SENDABLE, - ACK_FAILED, - - NUM_EVENTS -}; - -/** - * Context for the I2C FSM - */ -static struct fsm_context { - enum fsm_state state; - - /* XXX want to eliminate these */ - uint8_t command; - uint8_t status; - - uint8_t *data_ptr; - uint32_t data_count; - - size_t buffer_size; - uint8_t *buffer; -} context; - -/** - * Structure defining one FSM state and its outgoing transitions. - */ -struct fsm_transition { - void (*handler)(void); - enum fsm_state next_state[NUM_EVENTS]; -}; - -static bool i2c_command_received; - -static void fsm_event(enum fsm_event event); - -static void go_bad(void); -static void go_wait_master(void); - -static void go_wait_command(void); -static void go_receive_command(void); -static void go_receive_data(void); -static void go_handle_command(void); - -static void go_wait_send(void); -static void go_send_status(void); -static void go_send_buffer(void); - -/** - * The FSM state graph. - */ -static const struct fsm_transition fsm[NUM_STATES] = { - [BAD_PHASE] = { - .handler = go_bad, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - [WAIT_FOR_MASTER] = { - .handler = go_wait_master, - .next_state = { - [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, - [ADDRESSED_READ] = WAIT_TO_SEND, - }, - }, - - /* write from master*/ - [WAIT_FOR_COMMAND] = { - .handler = go_wait_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_COMMAND, - [STOP_RECEIVED] = WAIT_FOR_MASTER, - }, - }, - [RECEIVE_COMMAND] = { - .handler = go_receive_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [RECEIVE_DATA] = { - .handler = go_receive_data, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [HANDLE_COMMAND] = { - .handler = go_handle_command, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - /* buffer send */ - [WAIT_TO_SEND] = { - .handler = go_wait_send, - .next_state = { - [BYTE_SENDABLE] = SEND_STATUS, - }, - }, - [SEND_STATUS] = { - .handler = go_send_status, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, - [SEND_DATA] = { - .handler = go_send_buffer, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, -}; - - -/* debug support */ -#if 1 -struct fsm_logentry { - char kind; - uint32_t code; -}; - -#define LOG_ENTRIES 32 -static struct fsm_logentry fsm_log[LOG_ENTRIES]; -int fsm_logptr; -#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) -#define LOGx(_kind, _code) \ - do { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr = LOG_NEXT(fsm_logptr); \ - fsm_log[fsm_logptr].kind = 0; \ - } while(0) - -#define LOG(_kind, _code) \ - do {\ - if (fsm_logptr < LOG_ENTRIES) { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr++;\ - }\ - }while(0) - -#else -#define LOG(_kind, _code) -#endif - - -static void i2c_setclock(uint32_t frequency); - -/** - * Initialise I2C - * - */ -void -i2c_fsm_init(uint8_t *buffer, size_t buffer_size) -{ - /* save the buffer */ - context.buffer = buffer; - context.buffer_size = buffer_size; - - // initialise the FSM - context.status = 0; - context.command = 0; - context.state = BAD_PHASE; - fsm_event(AUTO); - -#if 0 - // enable the i2c block clock and reset it - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); - - // configure the i2c GPIOs - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); - - // set the peripheral clock to match the APB clock - rCR2 = STM32_PCLK1_FREQUENCY / 1000000; - - // configure for 100kHz operation - i2c_setclock(100000); - - // enable i2c - rCR1 = I2C_CR1_PE; -#endif -} - -/** - * Run the I2C FSM for some period. - * - * @return True if the buffer has been updated by a command. - */ -bool -i2c_fsm(void) -{ - uint32_t event; - int idle_iterations = 0; - - for (;;) { - // handle bus error states by discarding the current operation - if (rSR1 & I2C_SR1_BERR) { - context.state = WAIT_FOR_MASTER; - rSR1 = ~I2C_SR1_BERR; - } - - // we do not anticipate over/underrun errors as clock-stretching is enabled - - // fetch the most recent event - event = ((rSR2 << 16) | rSR1) & 0x00ffffff; - - // generate FSM events based on I2C events - switch (event) { - case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: - LOG('w', 0); - fsm_event(ADDRESSED_WRITE); - break; - - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: - LOG('r', 0); - fsm_event(ADDRESSED_READ); - break; - - case I2C_EVENT_SLAVE_BYTE_RECEIVED: - LOG('R', 0); - fsm_event(BYTE_RECEIVED); - break; - - case I2C_EVENT_SLAVE_STOP_DETECTED: - LOG('s', 0); - fsm_event(STOP_RECEIVED); - break; - - case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: - //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: - LOG('T', 0); - fsm_event(BYTE_SENDABLE); - break; - - case I2C_EVENT_SLAVE_ACK_FAILURE: - LOG('a', 0); - fsm_event(ACK_FAILED); - break; - - default: - idle_iterations++; -// if ((event) && (event != 0x00020000)) -// LOG('e', event); - break; - } - - /* if we have just received something, drop out and let the caller handle it */ - if (i2c_command_received) { - i2c_command_received = false; - return true; - } - - /* if we have done nothing recently, drop out and let the caller have a slice */ - if (idle_iterations > 1000) - return false; - } -} - -/** - * Update the FSM with an event - * - * @param event New event. - */ -static void -fsm_event(enum fsm_event event) -{ - // move to the next state - context.state = fsm[context.state].next_state[event]; - - LOG('f', context.state); - - // call the state entry handler - if (fsm[context.state].handler) { - fsm[context.state].handler(); - } -} - -static void -go_bad() -{ - LOG('B', 0); - fsm_event(AUTO); -} - -/** - * Wait for the master to address us. - * - */ -static void -go_wait_master() -{ - // We currently don't have a command byte. - // - context.command = '\0'; - - // The data pointer starts pointing to the start of the data buffer. - // - context.data_ptr = context.buffer; - - // The data count is either: - // - the size of the data buffer - // - some value less than or equal the size of the data buffer during a write or a read - // - context.data_count = context.buffer_size; - - // (re)enable the peripheral, clear the stop event flag in - // case we just finished receiving data - rCR1 |= I2C_CR1_PE; - - // clear the ACK failed flag in case we just finished sending data - rSR1 = ~I2C_SR1_AF; -} - -/** - * Prepare to receive a command byte. - * - */ -static void -go_wait_command() -{ - // NOP -} - -/** - * Command byte has been received, save it and prepare to handle the data. - * - */ -static void -go_receive_command() -{ - - // fetch the command byte - context.command = (uint8_t)rDR; - LOG('c', context.command); - -} - -/** - * Receive a data byte. - * - */ -static void -go_receive_data() -{ - uint8_t d; - - // fetch the byte - d = (uint8_t)rDR; - LOG('d', d); - - // if we have somewhere to put it, do so - if (context.data_count) { - *context.data_ptr++ = d; - context.data_count--; - } -} - -/** - * Handle a command once the host is done sending it to us. - * - */ -static void -go_handle_command() -{ - // presume we are happy with the command - context.status = 0; - - // make a note that the buffer contains a fresh command - i2c_command_received = true; - - // kick along to the next state - fsm_event(AUTO); -} - -/** - * Wait to be able to send the status byte. - * - */ -static void -go_wait_send() -{ - // NOP -} - -/** - * Send the status byte. - * - */ -static void -go_send_status() -{ - rDR = context.status; - LOG('?', context.status); -} - -/** - * Send a data or pad byte. - * - */ -static void -go_send_buffer() -{ - if (context.data_count) { - LOG('D', *context.data_ptr); - rDR = *(context.data_ptr++); - context.data_count--; - } else { - LOG('-', 0); - rDR = 0xff; - } -} - -/* cribbed directly from the NuttX master driver */ -static void -i2c_setclock(uint32_t frequency) -{ - uint16_t cr1; - uint16_t ccr; - uint16_t trise; - uint16_t freqmhz; - uint16_t speed; - - /* Disable the selected I2C peripheral to configure TRISE */ - - cr1 = rCR1; - rCR1 &= ~I2C_CR1_PE; - - /* Update timing and control registers */ - - freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); - ccr = 0; - - /* Configure speed in standard mode */ - - if (frequency <= 100000) { - /* Standard mode speed calculation */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); - - /* The CCR fault must be >= 4 */ - - if (speed < 4) { - /* Set the minimum allowed value */ - - speed = 4; - } - ccr |= speed; - - /* Set Maximum Rise Time for standard mode */ - - trise = freqmhz + 1; - - /* Configure speed in fast mode */ - } else { /* (frequency <= 400000) */ - /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ - -#ifdef CONFIG_I2C_DUTY16_9 - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); - - /* Set DUTY and fast speed bits */ - - ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); -#else - /* Fast mode speed calculation with Tlow/Thigh = 2 */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); - - /* Set fast speed bit */ - - ccr |= I2C_CCR_FS; -#endif - - /* Verify that the CCR speed value is nonzero */ - - if (speed < 1) { - /* Set the minimum allowed value */ - - speed = 1; - } - ccr |= speed; - - /* Set Maximum Rise Time for fast mode */ - - trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); - } - - /* Write the new values of the CCR and TRISE registers */ - - rCCR = ccr; - rTRISE = trise; - - /* Bit 14 of OAR1 must be configured and kept at 1 */ - - rOAR1 = I2C_OAR1_ONE); - - /* Re-enable the peripheral (or not) */ - - rCR1 = cr1; -} diff --git a/src/drivers/boards/px4iov2/module.mk b/src/drivers/boards/px4iov2/module.mk new file mode 100644 index 000000000..85f94e8be --- /dev/null +++ b/src/drivers/boards/px4iov2/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c \ + px4iov2_pwm_servo.c diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c new file mode 100644 index 000000000..09469d7e8 --- /dev/null +++ b/src/drivers/boards/px4iov2/px4iov2_init.c @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * 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 px4iov2_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "stm32_internal.h" +#include "px4iov2_internal.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + + /* configure GPIOs */ + + /* turn off - all leds are active low */ + stm32_gpiowrite(GPIO_LED1, true); + stm32_gpiowrite(GPIO_LED2, true); + stm32_gpiowrite(GPIO_LED3, true); + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - disable it by default */ + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + /* servo power enable is active low, and has a pull down resistor + * to keep it low during boot (since it may power the whole board.) + */ + stm32_gpiowrite(GPIO_SERVO_PWR_EN, false); + stm32_configgpio(GPIO_SERVO_PWR_EN); + + stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + + stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ + stm32_configgpio(GPIO_ADC_RSSI); + stm32_configgpio(GPIO_ADC_VSERVO); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); + stm32_configgpio(GPIO_SBUS_OUTPUT); + + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(GPIO_SBUS_OENABLE, true); + stm32_configgpio(GPIO_SBUS_OENABLE); + + + stm32_configgpio(GPIO_PPM); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_PWM1, false); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, false); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, false); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, false); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, false); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, false); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, false); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, false); + stm32_configgpio(GPIO_PWM8); + +// message("[boot] Successfully initialized px4iov2 gpios\n"); +} diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h new file mode 100755 index 000000000..c4c592fe4 --- /dev/null +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include +#include +#include + +/* these headers are not C++ safe */ +#include + + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) + +#define GPIO_LED_BLUE BOARD_GPIO_LED1 +#define GPIO_LED_AMBER BOARD_GPIO_LED2 +#define GPIO_LED_SAFETY BOARD_GPIO_LED3 + +/* Safety switch button *******************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) + + +/* Analog inputs **************************************************************/ + +#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) +/* the same rssi signal goes to both an adc and a timer input */ +#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) +/* floating pin */ +#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8) + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) + diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c new file mode 100644 index 000000000..5e1aafa49 --- /dev/null +++ b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 px4iov2_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; -- cgit v1.2.3 From 00daa29b1f6a2989e8d732aabf075a6a912ec3ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 12:37:33 -0700 Subject: Kill old iov2 board support code --- apps/drivers/boards/px4iov2/module.mk | 5 - apps/drivers/boards/px4iov2/px4iov2_init.c | 172 ------------------------- apps/drivers/boards/px4iov2/px4iov2_internal.h | 135 ------------------- 3 files changed, 312 deletions(-) delete mode 100644 apps/drivers/boards/px4iov2/module.mk delete mode 100644 apps/drivers/boards/px4iov2/px4iov2_init.c delete mode 100644 apps/drivers/boards/px4iov2/px4iov2_internal.h diff --git a/apps/drivers/boards/px4iov2/module.mk b/apps/drivers/boards/px4iov2/module.mk deleted file mode 100644 index d596ce4db..000000000 --- a/apps/drivers/boards/px4iov2/module.mk +++ /dev/null @@ -1,5 +0,0 @@ -# -# Board-specific startup code for the PX4IOv2 -# - -SRCS = px4iov2_init.c diff --git a/apps/drivers/boards/px4iov2/px4iov2_init.c b/apps/drivers/boards/px4iov2/px4iov2_init.c deleted file mode 100644 index 711bee425..000000000 --- a/apps/drivers/boards/px4iov2/px4iov2_init.c +++ /dev/null @@ -1,172 +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 px4iov2_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include - -#include "stm32_internal.h" -#include "px4iov2_internal.h" - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - - /* configure GPIOs */ - - /* turn off - all leds are active low */ - stm32_gpiowrite(BOARD_GPIO_LED1, true); - stm32_gpiowrite(BOARD_GPIO_LED2, true); - stm32_gpiowrite(BOARD_GPIO_LED3, true); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED1)); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED2)); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_LED3)); - - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_BTN_SAFETY)); - - /* spektrum power enable is active high - disable it by default */ - stm32_gpiowrite(BOARD_GPIO_SPEKTRUM_PWR_EN, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SPEKTRUM_PWR_EN)); - - /* servo power enable is active low, and has a pull down resistor - * to keep it low during boot (since it may power the whole board.) - */ - stm32_gpiowrite(BOARD_GPIO_SERVO_PWR_EN, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SERVO_PWR_EN)); - - stm32_configgpio(BOARD_GPIO_INPUT_PUP(BOARD_GPIO_SERVO_FAULT_DETECT)); - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_TIM_RSSI)); /* xxx alternate function */ - stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_RSSI)); - stm32_configgpio(BOARD_GPIO_INPUT_ANALOG(BOARD_GPIO_ADC_VSERVO)); - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_SBUS_INPUT)); /* xxx alternate function */ - - stm32_gpiowrite(BOARD_GPIO_SBUS_OUTPUT, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OUTPUT)); - /* sbus output enable is active low - disable it by default */ - stm32_gpiowrite(BOARD_GPIO_SBUS_OENABLE, true); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_SBUS_OENABLE)); - - - stm32_configgpio(BOARD_GPIO_INPUT_FLOAT(BOARD_GPIO_PPM)); /* xxx alternate function */ - - stm32_gpiowrite(BOARD_GPIO_PWM1, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM1)); - - stm32_gpiowrite(BOARD_GPIO_PWM2, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM2)); - - stm32_gpiowrite(BOARD_GPIO_PWM3, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM3)); - - stm32_gpiowrite(BOARD_GPIO_PWM4, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM4)); - - stm32_gpiowrite(BOARD_GPIO_PWM5, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM5)); - - stm32_gpiowrite(BOARD_GPIO_PWM6, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM6)); - - stm32_gpiowrite(BOARD_GPIO_PWM7, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM7)); - - stm32_gpiowrite(BOARD_GPIO_PWM8, false); - stm32_configgpio(BOARD_GPIO_OUTPUT(BOARD_GPIO_PWM8)); - -// message("[boot] Successfully initialized px4iov2 gpios\n"); - - return OK; -} diff --git a/apps/drivers/boards/px4iov2/px4iov2_internal.h b/apps/drivers/boards/px4iov2/px4iov2_internal.h deleted file mode 100644 index 9675c6f36..000000000 --- a/apps/drivers/boards/px4iov2/px4iov2_internal.h +++ /dev/null @@ -1,135 +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 px4iov2_internal.h - * - * PX4IOV2 internal definitions - */ - -#pragma once - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -__BEGIN_DECLS - -/* these headers are not C++ safe */ -#include - - -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/****************************************************************************** - * GPIOS - ******************************************************************************/ - -#define BOARD_GPIO_OUTPUT(pin) (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|(pin)) -#define BOARD_GPIO_INPUT_FLOAT(pin) (GPIO_INPUT|GPIO_CNF_INFLOAT|\ - GPIO_MODE_INPUT|(pin)) -#define BOARD_GPIO_INPUT_PUP(pin) (GPIO_INPUT|GPIO_CNF_INPULLUP|\ - GPIO_MODE_INPUT|(pin)) -#define BOARD_GPIO_INPUT_ANALOG(pin) (GPIO_INPUT|GPIO_CNF_ANALOGIN|\ - GPIO_MODE_INPUT|(pin)) - -/* LEDS **********************************************************************/ - -#define BOARD_GPIO_LED1 (GPIO_PORTB|GPIO_PIN14) -#define BOARD_GPIO_LED2 (GPIO_PORTB|GPIO_PIN15) -#define BOARD_GPIO_LED3 (GPIO_PORTB|GPIO_PIN13) - -#define BOARD_GPIO_LED_BLUE BOARD_GPIO_LED1 -#define BOARD_GPIO_LED_AMBER BOARD_GPIO_LED2 -#define BOARD_GPIO_LED_SAFETY BOARD_GPIO_LED3 - -/* Safety switch button *******************************************************/ - -#define BOARD_GPIO_BTN_SAFETY (GPIO_PORTB|GPIO_PIN5) - -/* Power switch controls ******************************************************/ - -#define BOARD_GPIO_SPEKTRUM_PWR_EN (GPIO_PORTC|GPIO_PIN13) - -#define BOARD_GPIO_SERVO_PWR_EN (GPIO_PORTC|GPIO_PIN15) - -#define BOARD_GPIO_SERVO_FAULT_DETECT (GPIO_PORTB|GPIO_PIN13) - - -/* Analog inputs **************************************************************/ - -#define BOARD_GPIO_ADC_VSERVO (GPIO_PORTA|GPIO_PIN4) -/* the same rssi signal goes to both an adc and a timer input */ -#define BOARD_GPIO_ADC_RSSI (GPIO_PORTA|GPIO_PIN5) -#define BOARD_GPIO_TIM_RSSI (GPIO_PORTA|GPIO_PIN12) - -/* PWM pins **************************************************************/ - -#define BOARD_GPIO_PPM (GPIO_PORTA|GPIO_PIN8) - -#define BOARD_GPIO_PWM1 (GPIO_PORTA|GPIO_PIN0) -#define BOARD_GPIO_PWM2 (GPIO_PORTA|GPIO_PIN1) -#define BOARD_GPIO_PWM3 (GPIO_PORTB|GPIO_PIN8) -#define BOARD_GPIO_PWM4 (GPIO_PORTB|GPIO_PIN9) -#define BOARD_GPIO_PWM5 (GPIO_PORTA|GPIO_PIN6) -#define BOARD_GPIO_PWM6 (GPIO_PORTA|GPIO_PIN7) -#define BOARD_GPIO_PWM7 (GPIO_PORTB|GPIO_PIN0) -#define BOARD_GPIO_PWM8 (GPIO_PORTB|GPIO_PIN1) - -/* SBUS pins *************************************************************/ - -#define BOARD_GPIO_SBUS_INPUT (GPIO_PORTB|GPIO_PIN11) -#define BOARD_GPIO_SBUS_OUTPUT (GPIO_PORTB|GPIO_PIN10) -#define BOARD_GPIO_SBUS_OENABLE (GPIO_PORTB|GPIO_PIN4) - -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - -#ifndef __ASSEMBLY__ - -#endif /* __ASSEMBLY__ */ - -__END_DECLS -- cgit v1.2.3 From 03eb16f874a8db7443e2b7f7a47d05c1a0c87357 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 12:47:34 -0700 Subject: Remove some naked command invocations. --- makefiles/module.mk | 2 +- makefiles/setup.mk | 2 ++ makefiles/upload.mk | 13 ++++++++----- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/makefiles/module.mk b/makefiles/module.mk index 86810627b..59c67f574 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -153,7 +153,7 @@ $(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).* $(MODULE_COMMAND_FILES): $(GLOBAL_DEPS) @$(REMOVE) -f $(exclude) @$(MKDIR) -p $(dir $@) - @echo "CMD: $(command)" + @$(ECHO) "CMD: $(command)" $(Q) $(TOUCH) $@ endif diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 8072ec791..111193093 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -72,6 +72,8 @@ export TOUCH = touch export MKDIR = mkdir export ECHO = echo export UNZIP_CMD = unzip +export PYTHON = python +export OPENOCD = openocd # # Host-specific paths, hacks and fixups diff --git a/makefiles/upload.mk b/makefiles/upload.mk index a1159d157..c9a317caf 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -25,7 +25,10 @@ endif all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu: $(BUNDLE) $(UPLOADER) - @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) + +upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) @python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) @@ -36,9 +39,9 @@ upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) JTAGCONFIG ?= interface/olimex-jtag-tiny.cfg upload-jtag-px4fmu: all - @echo Attempting to flash PX4FMU board via JTAG - @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown + @$(ECHO) Attempting to flash PX4FMU board via JTAG + $(Q) $(OPENOCD) -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown upload-jtag-px4io: all - @echo Attempting to flash PX4IO board via JTAG - @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown + @$(ECHO) Attempting to flash PX4IO board via JTAG + $(Q) $(OPENOCD) -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown -- cgit v1.2.3 From c6b7eb1224426d9ec2e6d59a3df4c7443449109a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:00:49 -0700 Subject: Remove obsoleted file. --- src/modules/attitude_estimator_ekf/Makefile | 57 ----------------------------- 1 file changed, 57 deletions(-) delete mode 100755 src/modules/attitude_estimator_ekf/Makefile diff --git a/src/modules/attitude_estimator_ekf/Makefile b/src/modules/attitude_estimator_ekf/Makefile deleted file mode 100755 index 46a54c660..000000000 --- a/src/modules/attitude_estimator_ekf/Makefile +++ /dev/null @@ -1,57 +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. -# -############################################################################ - -APPNAME = attitude_estimator_ekf -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -CXXSRCS = attitude_estimator_ekf_main.cpp - -CSRCS = attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c - - -# XXX this is *horribly* broken -INCLUDES += $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk -- cgit v1.2.3 From 301b600b0a0fbb816b692f53224d1ab6c5958741 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:22:36 -0700 Subject: Fix board name defines in v2 config. --- nuttx/configs/px4iov2/io/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig index c73f0df89..1eaf4f2d1 100755 --- a/nuttx/configs/px4iov2/io/defconfig +++ b/nuttx/configs/px4iov2/io/defconfig @@ -74,8 +74,8 @@ CONFIG_ARCH_ARM=y CONFIG_ARCH_CORTEXM3=y CONFIG_ARCH_CHIP="stm32" CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4io" -CONFIG_ARCH_BOARD_PX4IO=y +CONFIG_ARCH_BOARD="px4iov2" +CONFIG_ARCH_BOARD_PX4IOV2=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -- cgit v1.2.3 From 2423c54e0e5500a8a9e2ef482b95b351a4a6071e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:45:21 -0700 Subject: Build the right config for IOv2 --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 8f566a002..2a5abb741 100644 --- a/Makefile +++ b/Makefile @@ -140,9 +140,9 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) -$(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh) +$(ARCHIVE_DIR)%.export: configuration = $(if $(findstring px4io,$(board)),io,nsh) $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) - @echo %% Configuring NuttX for $(board) + @echo %% Configuring NuttX for $(board)/$(configuration) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) -- cgit v1.2.3 From 8f7200e0114d6bd9fcac7ec088b125e54761c2e0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 13:51:33 -0700 Subject: Frame up the configuration for the serial interface on IOv2 --- src/modules/px4iofirmware/module.mk | 11 +++++++++-- src/modules/px4iofirmware/px4io.c | 13 +++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6379366f4..016be6d3b 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -3,7 +3,6 @@ SRCS = adc.c \ controls.c \ dsm.c \ - i2c.c \ px4io.c \ registers.c \ safety.c \ @@ -16,4 +15,12 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ - \ No newline at end of file + +ifneq ($(CONFIG_ARCH_BOARD_PX4IO),) +SRCS += i2c.c +EXTRADEFINES += -DINTERFACE_I2C +endif +ifneq ($(CONFIG_ARCH_BOARD_PX4IOV2),) +#SRCS += serial.c +EXTRADEFINES += -DINTERFACE_SERIAL +endif diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc116..39f05c112 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -64,11 +64,6 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif - /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -159,10 +154,13 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 +#ifdef INTERFACE_I2C /* start the i2c handler */ i2c_init(); #endif +#ifdef INTERFACE_SERIAL + /* start the serial interface */ +#endif /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -223,12 +221,11 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -- cgit v1.2.3 From e67022f874f0fa091ed7dffd617c70c0c0253b5c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 28 Apr 2013 18:14:46 -0700 Subject: Serial interface for IOv2 --- src/modules/px4iofirmware/i2c.c | 15 ++-- src/modules/px4iofirmware/module.mk | 10 ++- src/modules/px4iofirmware/protocol.h | 39 ++++++++--- src/modules/px4iofirmware/px4io.c | 19 +++--- src/modules/px4iofirmware/px4io.h | 22 +++--- src/modules/px4iofirmware/serial.c | 129 +++++++++++++++++++++++++++++++++++ src/modules/systemlib/hx_stream.c | 42 ++++++++++-- src/modules/systemlib/hx_stream.h | 18 ++++- 8 files changed, 249 insertions(+), 45 deletions(-) create mode 100644 src/modules/px4iofirmware/serial.c diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b..10aeb5c9f 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,7 @@ static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +static void i2c_dump(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -92,7 +93,7 @@ enum { } direction; void -i2c_init(void) +interface_init(void) { debug("i2c init"); @@ -148,12 +149,18 @@ i2c_init(void) #endif } +void +interface_tick() +{ +} + /* reset the I2C bus used to recover from lockups */ -void i2c_reset(void) +void +i2c_reset(void) { rCR1 |= I2C_CR1_SWRST; rCR1 = 0; @@ -330,7 +337,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -void +static void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 016be6d3b..4dd1aa8d7 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -8,7 +8,6 @@ SRCS = adc.c \ safety.c \ sbus.c \ ../systemlib/up_cxxinitialize.c \ - ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ mixer.cpp \ ../systemlib/mixer/mixer.cpp \ @@ -16,11 +15,10 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -ifneq ($(CONFIG_ARCH_BOARD_PX4IO),) +ifeq ($(BOARD),px4io) SRCS += i2c.c -EXTRADEFINES += -DINTERFACE_I2C endif -ifneq ($(CONFIG_ARCH_BOARD_PX4IOV2),) -#SRCS += serial.c -EXTRADEFINES += -DINTERFACE_SERIAL +ifeq ($(BOARD),px4iov2) +SRCS += serial.c \ + ../systemlib/hx_stream.c endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 8d8b7e941..1a687bd2a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,7 @@ /** * @file protocol.h * - * PX4IO I2C interface protocol. + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -62,6 +62,27 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * PX4IO I2C interface notes: + * + * Register read/write operations are mapped directly to PX4IO register + * read/write operations. + * + * PX4IO Serial interface notes: + * + * The MSB of the page number is used to distinguish between read and + * write operations. If set, the operation is a write and additional + * data is expected to follow in the packet as for I2C writes. + * + * If clear, the packet is expected to contain a single byte giving the + * number of bytes to be read. PX4IO will respond with a packet containing + * the same header (page, offset) and the requested data. + * + * If a read is requested when PX4IO does not have buffer space to store + * the reply, the request will be dropped. PX4IO is always configured with + * enough space to receive one full-sized write and one read request, and + * to send one full-sized read response. + * */ #define PX4IO_CONTROL_CHANNELS 8 @@ -75,12 +96,14 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PAGE_WRITE (1<<7) + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ -#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ @@ -141,7 +164,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 64 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -160,13 +183,13 @@ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */ /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 67 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -178,10 +201,10 @@ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 39f05c112..385920d53 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -142,7 +142,7 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ POWER_SERVO(true); /* start the safety switch handler */ @@ -154,13 +154,11 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef INTERFACE_I2C - /* start the i2c handler */ - i2c_init(); -#endif -#ifdef INTERFACE_SERIAL - /* start the serial interface */ -#endif + /* start the FMU interface */ + interface_init(); + + /* add a performance counter for the interface */ + perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -203,6 +201,11 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); + /* kick the interface */ + perf_begin(interface_perf); + interface_tick(); + perf_end(interface_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..2077e6244 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -125,16 +125,16 @@ extern struct sys_state_s system_state; #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) #ifdef GPIO_ACC1_PWR_EN - #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) #endif #ifdef GPIO_ACC2_PWR_EN - #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) #endif #ifdef GPIO_RELAY1_EN - #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #endif #ifdef GPIO_RELAY2_EN - #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) #endif #define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) @@ -156,12 +156,11 @@ extern void mixer_handle_text(const void *buffer, size_t length); */ extern void safety_init(void); -#ifdef CONFIG_STM32_I2C1 /** * FMU communications */ -extern void i2c_init(void); -#endif +extern void interface_init(void); +extern void interface_tick(void); /** * Register space @@ -190,10 +189,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/* send a debug message to the console */ -extern void isr_debug(uint8_t level, const char *fmt, ...); - -#ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); -#endif +/** send a debug message to the console */ +extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 000000000..a12d58aca --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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 serial.c + * + * Serial communication for the PX4IO module. + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +//#define DEBUG +#include "px4io.h" + +static uint8_t tx_buf[66]; /* XXX hardcoded magic number */ + +static hx_stream_t if_stream; + +static void serial_callback(void *arg, const void *data, unsigned length); + +void +interface_init(void) +{ + + int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK); + if (fd < 0) { + debug("serial fail"); + return; + } + + /* configure serial port - XXX increase port speed? */ + struct termios t; + tcgetattr(fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(fd, TCSANOW, &t); + + /* allocate the HX stream we'll use for communication */ + if_stream = hx_stream_init(fd, serial_callback, NULL); + + /* XXX add stream stats counters? */ + + debug("serial init"); +} + +void +interface_tick() +{ + /* process incoming bytes */ + hx_stream_rx(if_stream); +} + +static void +serial_callback(void *arg, const void *data, unsigned length) +{ + const uint8_t *message = (const uint8_t *)data; + + /* malformed frame, ignore it */ + if (length < 2) + return; + + /* it's a write operation, pass it to the register API */ + if (message[0] & PX4IO_PAGE_WRITE) { + registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1], + (const uint16_t *)&message[2], + (length - 2) / 2); + + return; + } + + /* it's a read */ + uint16_t *registers; + unsigned count; + + tx_buf[0] = message[0]; + tx_buf[1] = message[1]; + + /* get registers for response, send an empty reply on error */ + if (registers_get(message[0], message[1], ®isters, &count) < 0) + count = 0; + + /* fill buffer with message */ +#define TX_MAX ((sizeof(tx_buf) - 2) / 2) + if (count > TX_MAX) + count = TX_MAX; + memcpy(&tx_buf[2], registers, count * 2); + + /* try to send the message */ + hx_stream_send(if_stream, tx_buf, count * 2 + 2); +} \ No newline at end of file diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8d77f14a8..88f7f762c 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -76,6 +76,7 @@ struct hx_stream { static void hx_tx_raw(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c); static int hx_rx_frame(hx_stream_t stream); +static bool hx_rx_char(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c) @@ -224,13 +225,13 @@ hx_stream_send(hx_stream_t stream, return 0; } -void -hx_stream_rx(hx_stream_t stream, uint8_t c) +static bool +hx_rx_char(hx_stream_t stream, uint8_t c) { /* frame end? */ if (c == FBO) { hx_rx_frame(stream); - return; + return true; } /* escaped? */ @@ -241,10 +242,43 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } else if (c == CEO) { /* now escaped, ignore the byte */ stream->escaped = true; - return; + return false; } /* save for later */ if (stream->frame_bytes < sizeof(stream->buf)) stream->buf[stream->frame_bytes++] = c; + + return false; +} + +void +hx_stream_rx_char(hx_stream_t stream, uint8_t c) +{ + hx_rx_char(stream, c); +} + +int +hx_stream_rx(hx_stream_t stream) +{ + uint16_t buf[16]; + ssize_t len; + + /* read bytes */ + len = read(stream->fd, buf, sizeof(buf)); + if (len <= 0) { + + /* nonblocking stream and no data */ + if (errno == EWOULDBLOCK) + return 0; + + /* error or EOF */ + return -errno; + } + + /* process received characters */ + for (int i = 0; i < len; i++) + hx_rx_char(stream, buf[i]); + + return 0; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index 128689953..be4850f74 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -114,9 +114,25 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream, * @param stream A handle returned from hx_stream_init. * @param c The character to process. */ -__EXPORT extern void hx_stream_rx(hx_stream_t stream, +__EXPORT extern void hx_stream_rx_char(hx_stream_t stream, uint8_t c); +/** + * Handle received bytes from the stream. + * + * Note that this interface should only be used with blocking streams + * when it is OK for the call to block until a frame is received. + * + * When used with a non-blocking stream, it will typically return + * immediately, or after handling a received frame. + * + * @param stream A handle returned from hx_stream_init. + * @return -errno on error, nonzero if a frame + * has been received, or if not enough + * bytes are available to complete a frame. + */ +__EXPORT extern int hx_stream_rx(hx_stream_t stream); + __END_DECLS #endif -- cgit v1.2.3 From 7570d9082ca4291b598f3e34be291189678685ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 17:41:25 +0400 Subject: Use common accel calibration. Cleanup. --- apps/position_estimator_inav/Makefile | 5 - .../acceleration_transform.c | 130 --------------- .../acceleration_transform.h | 19 --- .../position_estimator_inav_main.c | 185 +++------------------ .../position_estimator_inav_params.c | 53 ------ .../position_estimator_inav_params.h | 19 --- 6 files changed, 22 insertions(+), 389 deletions(-) delete mode 100644 apps/position_estimator_inav/acceleration_transform.c delete mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 192ec1825..566bb9f43 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -39,9 +39,4 @@ APPNAME = position_estimator_inav PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 -CSRCS = position_estimator_inav_main.c \ - position_estimator_inav_params.c \ - kalman_filter_inertial.c \ - acceleration_transform.c - include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c deleted file mode 100644 index 3aba9b403..000000000 --- a/apps/position_estimator_inav/acceleration_transform.c +++ /dev/null @@ -1,130 +0,0 @@ -/* - * acceleration_transform.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - * - * Transform acceleration vector to true orientation and scale - * - * * * * Model * * * - * accel_corr = accel_T * (accel_raw - accel_offs) - * - * accel_corr[3] - fully corrected acceleration vector in UAV frame - * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform - * accel_raw[3] - raw acceleration vector - * accel_offs[3] - true acceleration offset vector, don't use sensors offset because - * it's caused not only by real offset but also by sensor rotation - * - * * * * Calibration * * * - * - * Reference vectors - * accel_corr_ref[6][3] = [ g 0 0 ] // positive x - * | -g 0 0 | // negative x - * | 0 g 0 | // positive y - * | 0 -g 0 | // negative y - * | 0 0 g | // positive z - * [ 0 0 -g ] // negative z - * accel_raw_ref[6][3] - * - * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 - * - * 6 reference vectors * 3 axes = 18 equations - * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants - * - * Find accel_offs - * - * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 - * - * - * Find accel_T - * - * 9 unknown constants - * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations - * - * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 - * - * Solve separate system for each row of accel_T: - * - * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 - * - * A x = b - * - * x = [ accel_T[0][i] ] - * | accel_T[1][i] | - * [ accel_T[2][i] ] - * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough - * | accel_corr_ref[2][i] | - * [ accel_corr_ref[4][i] ] - * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 - * - * Matrix A is common for all three systems: - * A = [ a[0][0] a[0][1] a[0][2] ] - * | a[2][0] a[2][1] a[2][2] | - * [ a[4][0] a[4][1] a[4][2] ] - * - * x = A^-1 b - * - * accel_T = A^-1 * g - * g = 9.81 - */ - -#include "acceleration_transform.h" - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]) { - for (int i = 0; i < 3; i++) { - accel_corr[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); - } - } -} - -int mat_invert3(float src[3][3], float dst[3][3]) { - float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) - return -1; // Singular matrix - dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; - dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; - dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; - dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; - dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; - dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; - dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; - dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; - dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - return 0; -} - -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g) { - /* calculate raw offsets */ - for (int i = 0; i < 3; i++) { - accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) - + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); - } - /* fill matrix A for linear equations system*/ - float mat_A[3][3]; - memset(mat_A, 0, sizeof(mat_A)); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); - mat_A[i][j] = a; - } - } - /* calculate inverse matrix for A */ - float mat_A_inv[3][3]; - mat_invert3(mat_A, mat_A_inv); - for (int i = 0; i < 3; i++) { - /* copy results to accel_T */ - for (int j = 0; j < 3; j++) { - /* simplify matrices mult because b has only one non-zero element == g at index i */ - accel_T[j][i] = mat_A_inv[j][i] * g; - } - } - return 0; -} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h deleted file mode 100644 index 4d1299db5..000000000 --- a/apps/position_estimator_inav/acceleration_transform.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * acceleration_transform.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#ifndef ACCELERATION_TRANSFORM_H_ -#define ACCELERATION_TRANSFORM_H_ - -#include - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]); - -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g); - -#endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 6e8c0ab5f..071ec6ad4 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -62,22 +62,19 @@ #include #include #include +#include #include #include "position_estimator_inav_params.h" #include "kalman_filter_inertial.h" -#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); -void do_accelerometer_calibration(); - int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); @@ -89,18 +86,18 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -136,136 +133,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } - if (!strcmp(argv[1], "calibrate")) { - do_accelerometer_calibration(); - exit(0); - } - usage("unrecognized command"); exit(1); } -void wait_for_input() { - printf("press any key to continue, 'Q' to abort\n"); - while (true ) { - int c = getchar(); - if (c == 'q' || c == 'Q') { - exit(0); - } else { - return; - } - } -} - -void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], - int samples) { - printf("[position_estimator_inav] measuring...\n"); - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; - int count = 0; - int32_t accel_sum[3] = { 0, 0, 0 }; - while (count < samples) { - int poll_ret = poll(fds, 1, 1000); - if (poll_ret == 1) { - struct sensor_combined_s sensor; - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - accel_sum[0] += sensor.accelerometer_raw[0]; - accel_sum[1] += sensor.accelerometer_raw[1]; - accel_sum[2] += sensor.accelerometer_raw[2]; - count++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - printf("[position_estimator_inav] no accelerometer data for 1s\n"); - exit(1); - } else { - printf("[position_estimator_inav] poll error\n"); - exit(1); - } - } - for (int i = 0; i < 3; i++) { - accel_avg[i] = (accel_sum[i] + count / 2) / count; - } - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], - accel_avg[1], accel_avg[2]); -} - -void do_accelerometer_calibration() { - printf("[position_estimator_inav] calibration started\n"); - const int calibration_samples = 1000; - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int16_t accel_raw_ref[6][3]; // Reference measurements - printf("[position_estimator_inav] place vehicle level, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on it's back, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on right side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on left side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose down, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose up, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), - calibration_samples); - close(sensor_combined_sub); - printf("[position_estimator_inav] reference data collection done\n"); - - int16_t accel_offs[3]; - float accel_T[3][3]; - int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, - const_earth_gravity); - if (res != 0) { - printf( - "[position_estimator_inav] calibration parameters calculation error\n"); - exit(1); - - } - printf( - "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", - accel_offs[0], accel_offs[1], accel_offs[2]); - printf( - "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", - accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], - accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], - accel_T[2][2]); - int32_t accel_offs_int32[3] = - { accel_offs[0], accel_offs[1], accel_offs[2] }; - - if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) - || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) - || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) - || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) - || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) - || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) - || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) - || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) - || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) - || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) - || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { - printf("[position_estimator_inav] setting parameters failed\n"); - exit(1); - } - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - if (save_ret != 0) { - printf( - "[position_estimator_inav] auto-save of parameters to storage failed\n"); - exit(1); - } - printf("[position_estimator_inav] calibration done\n"); -} - /**************************************************************************** * main ****************************************************************************/ @@ -277,18 +148,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float x_est[3] = { 0.0f, 0.0f, 0.0f }; - static float y_est[3] = { 0.0f, 0.0f, 0.0f }; - static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est[3] = { 0.0f, 0.0f, 0.0f }; + float y_est[3] = { 0.0f, 0.0f, 0.0f }; + float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ - float baro_alt0 = 0.0f; /* to determin while start up */ + float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0d; //[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - static double alt_current = 0.0d; //[m] above MSL + static double lat_current = 0.0; //[°]] --> 47.0 + static double lon_current = 0.0; //[°]] -->8.5 + static double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -473,26 +343,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { float dt = (t - last_time) / 1000000.0; last_time = t; if (att.R_valid) { - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); - } - } - accel_NED[2] += const_earth_gravity; - /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - /* the inverse of a rotation matrix is its transpose, just swap i and j */ - accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; /* kalman filter prediction */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ @@ -531,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = accel_offs_est[0]; - pos.vx = accel_offs_est[1]; - pos.y = accel_offs_est[2]; + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 8cd9844c7..bb2b01481 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,7 +39,6 @@ */ #include "position_estimator_inav_params.h" -#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -52,22 +51,6 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); - -PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); - -PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); - int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -78,21 +61,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); - h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); - - h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); - h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); - h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); - - h->acc_t_00 = param_find("INAV_ACC_T_00"); - h->acc_t_01 = param_find("INAV_ACC_T_01"); - h->acc_t_02 = param_find("INAV_ACC_T_02"); - h->acc_t_10 = param_find("INAV_ACC_T_10"); - h->acc_t_11 = param_find("INAV_ACC_T_11"); - h->acc_t_12 = param_find("INAV_ACC_T_12"); - h->acc_t_20 = param_find("INAV_ACC_T_20"); - h->acc_t_21 = param_find("INAV_ACC_T_21"); - h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } @@ -106,26 +74,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_w, &(p->acc_offs_w)); - - /* read int32 and cast to int16 */ - int32_t t; - param_get(h->acc_offs_0, &t); - p->acc_offs[0] = t; - param_get(h->acc_offs_1, &t); - p->acc_offs[1] = t; - param_get(h->acc_offs_2, &t); - p->acc_offs[2] = t; - - param_get(h->acc_t_00, &(p->acc_T[0][0])); - param_get(h->acc_t_01, &(p->acc_T[0][1])); - param_get(h->acc_t_02, &(p->acc_T[0][2])); - param_get(h->acc_t_10, &(p->acc_T[1][0])); - param_get(h->acc_t_11, &(p->acc_T[1][1])); - param_get(h->acc_t_12, &(p->acc_T[1][2])); - param_get(h->acc_t_20, &(p->acc_T[2][0])); - param_get(h->acc_t_21, &(p->acc_T[2][1])); - param_get(h->acc_t_22, &(p->acc_T[2][2])); - return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 2c6fb3df2..eaa1bbc95 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,9 +43,6 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; - float acc_offs_w; - int16_t acc_offs[3]; - float acc_T[3][3]; }; struct position_estimator_inav_param_handles { @@ -57,22 +54,6 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; - - param_t acc_offs_w; - - param_t acc_offs_0; - param_t acc_offs_1; - param_t acc_offs_2; - - param_t acc_t_00; - param_t acc_t_01; - param_t acc_t_02; - param_t acc_t_10; - param_t acc_t_11; - param_t acc_t_12; - param_t acc_t_20; - param_t acc_t_21; - param_t acc_t_22; }; /** -- cgit v1.2.3 From 861a21ef7559386d8301c6b8860a13ac2fc0ef64 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 19:02:32 +0400 Subject: Position estimation using accel+GPS implemented --- .../position_estimator_inav_main.c | 51 ++++++++++++++++------ .../position_estimator_inav_params.c | 35 +++++++++++---- .../position_estimator_inav_params.h | 10 ++++- 3 files changed, 74 insertions(+), 22 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 071ec6ad4..2b485f895 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -261,6 +261,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; + bool gps_updated = false; + float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ @@ -322,14 +325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); /* Project gps lat lon (Geographic coordinate system) to plane */ map_projection_project(((double) (gps.lat)) * 1e-7, ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } @@ -352,7 +354,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter prediction */ + + /* kalman filter for altitude */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // position, acceleration @@ -367,8 +370,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); + kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + } + + if (params.use_gps) { + /* kalman filter for position */ + kalman_filter_inertial_predict(dt, x_est); + kalman_filter_inertial_predict(dt, y_est); + /* prepare vectors for kalman filter correction */ + float x_meas[2]; // position, acceleration + float y_meas[2]; // position, acceleration + bool use_xy[2] = { false, false }; + if (gps_updated) { + x_meas[0] = local_pos_gps[0]; + y_meas[0] = local_pos_gps[1]; + use_xy[0] = true; + } + if (accelerometer_updated) { + x_meas[1] = accel_NED[0]; + y_meas[1] = accel_NED[1]; + use_xy[1] = true; + } + if (use_xy[0] || use_xy[1]) { + /* correction */ + kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + } } } if (verbose_mode) { @@ -390,10 +417,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; - pos.vy = 0.0f; + pos.x = x_est[0]; + pos.vx = x_est[1]; + pos.y = y_est[0]; + pos.vy = y_est[1]; pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); @@ -402,9 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { && (isfinite(pos.vy)) && (isfinite(pos.z)) && (isfinite(pos.vz))) { - orb_publish(ORB_ID( - vehicle_local_position), vehicle_local_position_pub, - &pos); + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index bb2b01481..8466bcd0a 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -40,8 +40,6 @@ #include "position_estimator_inav_params.h" -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ PARAM_DEFINE_INT32(INAV_USE_GPS, 0); PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); @@ -51,6 +49,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -61,18 +66,32 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->k_pos_00 = param_find("INAV_K_POS_00"); + h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_10 = param_find("INAV_K_POS_10"); + h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_20 = param_find("INAV_K_POS_20"); + h->k_pos_21 = param_find("INAV_K_POS_21"); + return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->use_gps, &(p->use_gps)); - param_get(h->k_alt_00, &(p->k[0][0])); - param_get(h->k_alt_01, &(p->k[0][1])); - param_get(h->k_alt_10, &(p->k[1][0])); - param_get(h->k_alt_11, &(p->k[1][1])); - param_get(h->k_alt_20, &(p->k[2][0])); - param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->k_alt_00, &(p->k_alt[0][0])); + param_get(h->k_alt_01, &(p->k_alt[0][1])); + param_get(h->k_alt_10, &(p->k_alt[1][0])); + param_get(h->k_alt_11, &(p->k_alt[1][1])); + param_get(h->k_alt_20, &(p->k_alt[2][0])); + param_get(h->k_alt_21, &(p->k_alt[2][1])); + + param_get(h->k_pos_00, &(p->k_pos[0][0])); + param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_10, &(p->k_pos[1][0])); + param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_20, &(p->k_pos[2][0])); + param_get(h->k_pos_21, &(p->k_pos[2][1])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index eaa1bbc95..8cdc2e10f 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -42,7 +42,8 @@ struct position_estimator_inav_params { int use_gps; - float k[3][2]; + float k_alt[3][2]; + float k_pos[3][2]; }; struct position_estimator_inav_param_handles { @@ -54,6 +55,13 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; + + param_t k_pos_00; + param_t k_pos_01; + param_t k_pos_10; + param_t k_pos_11; + param_t k_pos_20; + param_t k_pos_21; }; /** -- cgit v1.2.3 From c21ba6c5007530f22db704924e79e9bb1ea79130 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 May 2013 12:32:07 +0400 Subject: position_estimator_inav added to config_px4fmu_default.mk --- makefiles/config_px4fmu_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index ae62b7034..23700b0ac 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -64,6 +64,7 @@ MODULES += modules/attitude_estimator_ekf MODULES += modules/position_estimator_mc MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav # # Vehicle Control -- cgit v1.2.3 From 5842c2212334876979f329b9b6bceba9609d91af Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 May 2013 19:47:38 +0400 Subject: Use GPS velocity in position estimator --- .../kalman_filter_inertial.c | 17 +++++++++++++++- .../kalman_filter_inertial.h | 4 +++- .../position_estimator_inav_main.c | 23 ++++++++++++---------- .../position_estimator_inav_params.c | 9 +++++++++ .../position_estimator_inav_params.h | 5 ++++- 5 files changed, 45 insertions(+), 13 deletions(-) diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c index 64031ee7b..390e1b720 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c @@ -12,7 +12,7 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { x[1] += x[2] * dt; } -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; // y = z - H x y[0] = z[0] - x[0]; @@ -25,3 +25,18 @@ void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool u } } } + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { + float y[2]; + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[1]; + y[2] = z[2] - x[2]; + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 3; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h index 3e5134c33..d34f58298 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.h @@ -9,4 +9,6 @@ void kalman_filter_inertial_predict(float dt, float x[3]); -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2b485f895..6ecdfe01d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -370,7 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); } if (params.use_gps) { @@ -378,23 +378,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_predict(dt, x_est); kalman_filter_inertial_predict(dt, y_est); /* prepare vectors for kalman filter correction */ - float x_meas[2]; // position, acceleration - float y_meas[2]; // position, acceleration - bool use_xy[2] = { false, false }; + float x_meas[3]; // position, velocity, acceleration + float y_meas[3]; // position, velocity, acceleration + bool use_xy[3] = { false, false, false }; if (gps_updated) { x_meas[0] = local_pos_gps[0]; y_meas[0] = local_pos_gps[1]; + x_meas[1] = gps.vel_n_m_s; + y_meas[1] = gps.vel_e_m_s; use_xy[0] = true; + use_xy[1] = true; } if (accelerometer_updated) { - x_meas[1] = accel_NED[0]; - y_meas[1] = accel_NED[1]; - use_xy[1] = true; + x_meas[2] = accel_NED[0]; + y_meas[2] = accel_NED[1]; + use_xy[2] = true; } - if (use_xy[0] || use_xy[1]) { + if (use_xy[0] || use_xy[2]) { /* correction */ - kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); } } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8466bcd0a..20142b70c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -51,10 +51,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -68,10 +71,13 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_pos_00 = param_find("INAV_K_POS_00"); h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_02 = param_find("INAV_K_POS_02"); h->k_pos_10 = param_find("INAV_K_POS_10"); h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_12 = param_find("INAV_K_POS_12"); h->k_pos_20 = param_find("INAV_K_POS_20"); h->k_pos_21 = param_find("INAV_K_POS_21"); + h->k_pos_22 = param_find("INAV_K_POS_22"); return OK; } @@ -88,10 +94,13 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_pos_00, &(p->k_pos[0][0])); param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_02, &(p->k_pos[0][2])); param_get(h->k_pos_10, &(p->k_pos[1][0])); param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_12, &(p->k_pos[1][2])); param_get(h->k_pos_20, &(p->k_pos[2][0])); param_get(h->k_pos_21, &(p->k_pos[2][1])); + param_get(h->k_pos_22, &(p->k_pos[2][2])); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 8cdc2e10f..c0e0042b6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,7 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k_alt[3][2]; - float k_pos[3][2]; + float k_pos[3][3]; }; struct position_estimator_inav_param_handles { @@ -58,10 +58,13 @@ struct position_estimator_inav_param_handles { param_t k_pos_00; param_t k_pos_01; + param_t k_pos_02; param_t k_pos_10; param_t k_pos_11; + param_t k_pos_12; param_t k_pos_20; param_t k_pos_21; + param_t k_pos_22; }; /** -- cgit v1.2.3 From cb1fbecd09d0e98ac11a18342f8670d7eb71ec47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 May 2013 12:25:13 +0200 Subject: Merged master from main repo --- makefiles/config_px4fmuv2_default.mk | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index ab1f19314..26c249901 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -74,15 +74,19 @@ MODULES += modules/multirotor_pos_control MODULES += modules/sdlog # -# Libraries +# Library modules # MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib -MODULES += modules/mathlib/CMSIS MODULES += modules/controllib MODULES += modules/uORB +# +# Libraries +# +LIBRARIES += modules/mathlib/CMSIS + # # Demo apps # -- cgit v1.2.3 From 78d29045c4da2d5ed4cea50fc2184b57dea01951 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 May 2013 20:12:16 +0200 Subject: Fix configuration selection for px4iov2; still doesn't build completely, but it's better. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index dea344390..ad10ed7c8 100644 --- a/Makefile +++ b/Makefile @@ -145,7 +145,7 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) -$(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh) +$(ARCHIVE_DIR)%.export: configuration = $(if $(filter px4io px4iov2,$(board)),io,nsh) $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) @echo %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) -- cgit v1.2.3 From 308ec6001a2e1ac31ea818b1d482a34b8ed0099b Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 May 2013 22:09:00 +0200 Subject: Add serial read-length handling. --- src/modules/px4iofirmware/protocol.h | 11 +++++++---- src/modules/px4iofirmware/serial.c | 8 ++++++-- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 29107f79f..6cdf86b2b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,8 @@ /** * @file protocol.h * - * PX4IO interface protocol. + * PX4IO interface protocol + * ======================== * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -63,19 +64,21 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * PX4IO I2C interface notes: + * PX4IO I2C interface notes + * ------------------------- * * Register read/write operations are mapped directly to PX4IO register * read/write operations. * - * PX4IO Serial interface notes: + * PX4IO Serial interface notes + * ---------------------------- * * The MSB of the page number is used to distinguish between read and * write operations. If set, the operation is a write and additional * data is expected to follow in the packet as for I2C writes. * * If clear, the packet is expected to contain a single byte giving the - * number of bytes to be read. PX4IO will respond with a packet containing + * number of registers to be read. PX4IO will respond with a packet containing * the same header (page, offset) and the requested data. * * If a read is requested when PX4IO does not have buffer space to store diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index a12d58aca..bf9456e94 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -107,7 +107,9 @@ serial_callback(void *arg, const void *data, unsigned length) return; } - /* it's a read */ + /* it's a read - must contain length byte */ + if (length != 3) + return; uint16_t *registers; unsigned count; @@ -118,10 +120,12 @@ serial_callback(void *arg, const void *data, unsigned length) if (registers_get(message[0], message[1], ®isters, &count) < 0) count = 0; - /* fill buffer with message */ + /* fill buffer with message, limited by length */ #define TX_MAX ((sizeof(tx_buf) - 2) / 2) if (count > TX_MAX) count = TX_MAX; + if (count > message[2]) + count = message[2]; memcpy(&tx_buf[2], registers, count * 2); /* try to send the message */ -- cgit v1.2.3 From 4bf49cfc35e86528a7b321dc0b8fb55c36fad510 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Jun 2013 19:28:25 +0400 Subject: multirotor_pos_control cleanup --- .../multirotor_pos_control/position_control.c | 235 --------------------- .../multirotor_pos_control/position_control.h | 50 ----- 2 files changed, 285 deletions(-) delete mode 100644 src/modules/multirotor_pos_control/position_control.c delete mode 100644 src/modules/multirotor_pos_control/position_control.h diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6..000000000 --- a/src/modules/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc34..000000000 --- a/src/modules/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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 multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ -- cgit v1.2.3 From 8567134d64f5d8e3c7aba7cebfdf56ffe56b44ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 19:41:41 +0200 Subject: Made pwm command sending continously, improved failsafe logic --- src/modules/px4iofirmware/mixer.cpp | 16 ++++++------- src/systemcmds/pwm/pwm.c | 48 ++++++++++++++++++++++++++++++++----- 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..123eb6778 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -98,7 +98,7 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { @@ -112,12 +112,11 @@ mixer_tick(void) * Decide which set of controls we're using. */ - /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ + /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -196,10 +195,9 @@ mixer_tick(void) bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ); if (should_arm && !mixer_servos_armed) { diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index e150b5a74..570ca6aa9 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[]) } /* iterate remaining arguments */ - unsigned channel = 0; + unsigned nchannels = 0; + unsigned channel[8] = {0}; while (argc--) { const char *arg = argv[0]; argv++; @@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[]) } 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++; + channel[nchannels] = pwm_value; + nchannels++; + + if (nchannels >= sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + continue; } - usage("unrecognised option"); + usage("unrecognized option"); } /* print verbose info */ @@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[]) } fflush(stdout); } + + /* perform PWM output */ + if (nchannels) { + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (int i = 0; i < nchannels; i++) { + ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } + + /* abort on user request */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + close(console); + exit(0); + } + } + + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + } + exit(0); } \ No newline at end of file -- cgit v1.2.3 From d2c5990d6f0b1c3f4183a193c1c51250cbdfa127 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 12:41:47 +0200 Subject: Fixed pwm count check --- src/systemcmds/pwm/pwm.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 570ca6aa9..619bd2c78 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -205,12 +205,12 @@ pwm_main(int argc, char *argv[]) } unsigned pwm_value = strtol(arg, &ep, 0); if (*ep == '\0') { + if (nchannels > sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + channel[nchannels] = pwm_value; nchannels++; - if (nchannels >= sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); - continue; } usage("unrecognized option"); -- cgit v1.2.3 From 4ef87206eccd292eb5111bba7d5f39dd03f7e20c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:03:49 +0200 Subject: Code formatting and warning fixes --- src/drivers/blinkm/blinkm.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a5..9fed1149d 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -486,15 +486,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { -- cgit v1.2.3 From b12678014ff9b500912ec44f6f9c771af3bdd217 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:04:13 +0200 Subject: Fixed chan count logic --- src/systemcmds/pwm/pwm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 619bd2c78..c42a36c7f 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -274,7 +274,7 @@ pwm_main(int argc, char *argv[]) /* abort on user request */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); -- cgit v1.2.3 From 1deced7629e7d140a931c42657f75da512696c7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:09:09 +0200 Subject: Added safety status feedback, disallow arming of a rotary wing with engaged safety --- src/drivers/px4io/px4io.cpp | 33 ++++++++++- src/modules/commander/commander.c | 82 ++++++++++++++++++---------- src/modules/commander/state_machine_helper.c | 35 +++++++++++- src/modules/commander/state_machine_helper.h | 4 ++ src/modules/uORB/objects_common.cpp | 4 ++ src/modules/uORB/topics/actuator_controls.h | 7 ++- src/modules/uORB/topics/safety.h | 60 ++++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 189 insertions(+), 38 deletions(-) create mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe..bc65c4f25 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -161,6 +162,7 @@ private: orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///< mixed outputs actuator_controls_effective_s _controls_effective; ///< effective controls @@ -883,6 +885,25 @@ PX4IO::io_handle_status(uint16_t status) _status = status; } + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + safety.status = SAFETY_STATUS_UNLOCKED; + } else { + safety.status = SAFETY_STATUS_SAFE; + } + + /* lazily publish the safety status */ + if (_to_safety > 0) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + return ret; } @@ -912,7 +933,7 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - + /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; @@ -946,6 +967,7 @@ PX4IO::io_get_status() _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; } @@ -1273,7 +1295,7 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), @@ -1634,6 +1656,11 @@ test(void) if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); @@ -1682,7 +1709,7 @@ test(void) /* Check if user wants to quit */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04..937c515e6 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -1281,6 +1282,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ + current_status.flag_safety_present = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1377,6 +1380,12 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + safety.status = SAFETY_STATUS_NOT_PRESENT; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1396,6 +1405,39 @@ int commander_thread_main(int argc, char *argv[]) /* Get current values */ bool new_data; + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + orb_check(sp_man_sub, &new_data); if (new_data) { @@ -1431,36 +1473,17 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } + orb_check(safety_sub, &new_data); - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); + if (new_data) { + /* got safety change */ + orb_copy(ORB_ID(safety), safety_sub, &safety); - } + /* handle it */ + current_status.flag_safety_present = (safety.status != SAFETY_STATUS_NOT_PRESENT); + + if (current_status.flag_safety_present) + current_status.flag_safety_safe = (safety.status == SAFETY_STATUS_SAFE); } /* update global position estimate */ @@ -1699,7 +1722,8 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + orb_check(ORB_ID(vehicle_gps_position), &new_data); + if (new_data) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..ac603abfd 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -65,6 +65,18 @@ static const char *system_state_txt[] = { }; +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status); +} + /** * Transition from one state to another */ @@ -513,6 +525,25 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + + /* reject arming if safety status is wrong */ + if (current_status->flag_safety_present) { + + /* + * The operator should not touch the vehicle if + * its armed, so we don't allow arming in the + * first place + */ + if (is_rotary_wing(current_status)) { + + /* safety is in safe position, disallow arming */ + if (current_status->flag_safety_safe) { + mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + } + + } + } + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } @@ -538,9 +569,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + if (is_rotary_wing(current_status)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc729..95b48d326 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,10 @@ #include #include +bool is_multirotor(const struct vehicle_status_s *current_status); + +bool is_rotary_wing(const struct vehicle_status_s *current_status); + /** * Switch to new state with no checking. * diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2..e2df31651 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -154,3 +154,7 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); + +/* status of the system safety device */ +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..745c5bc87 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -68,9 +68,10 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool throttle_locked; /**< Set to true if the trottle is locked to zero */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; ORB_DECLARE(actuator_armed); diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 000000000..19e8e8d45 --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 safety.h + * + * Status of an attached safety device + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +enum SAFETY_STATUS { + SAFETY_STATUS_NOT_PRESENT, + SAFETY_STATUS_SAFE, + SAFETY_STATUS_UNLOCKED +}; + +struct safety_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + enum SAFETY_STATUS status; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(safety); + +#endif /* TOPIC_SAFETY_H */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f6..07660614b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -214,6 +214,8 @@ struct vehicle_status_s bool flag_valid_launch_position; /**< indicates a valid launch position */ bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool flag_safety_present; /**< indicates that a safety switch is present */ + bool flag_safety_safe; /**< safety switch is in safe position */ }; /** -- cgit v1.2.3 From 8b67f88331a9dc65e5c947da177701317d77f8bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:12:17 +0200 Subject: Play warning tune --- src/modules/commander/state_machine_helper.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ac603abfd..e18c0edc3 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -49,6 +49,7 @@ #include #include "state_machine_helper.h" +#include "commander.h" static const char *system_state_txt[] = { "SYSTEM_STATE_PREFLIGHT", @@ -539,6 +540,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* safety is in safe position, disallow arming */ if (current_status->flag_safety_safe) { mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + + /* play warning tune */ + tune_error(); } } -- cgit v1.2.3 From 1028bd932cfd08366dd0dcb8c189ebcf88cce53a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Jun 2013 07:39:12 +0200 Subject: Extended vehicle detection --- src/modules/commander/state_machine_helper.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index e18c0edc3..0a6cfd0b5 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,12 +70,14 @@ bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status); + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } /** -- cgit v1.2.3 From 4256e43de7ea4c9cad98e8bfc9a811310bfb3d77 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Jun 2013 23:16:04 +0400 Subject: Complete position estimator implemented (GPS + Baro + Accel) --- .../position_estimator_inav/inertial_filter.c | 28 +++ .../position_estimator_inav/inertial_filter.h | 13 ++ .../kalman_filter_inertial.c | 42 ---- .../kalman_filter_inertial.h | 14 -- src/modules/position_estimator_inav/module.mk | 2 +- .../position_estimator_inav_main.c | 242 +++++++++++---------- .../position_estimator_inav_params.c | 76 ++----- .../position_estimator_inav_params.h | 31 +-- 8 files changed, 207 insertions(+), 241 deletions(-) create mode 100644 src/modules/position_estimator_inav/inertial_filter.c create mode 100644 src/modules/position_estimator_inav/inertial_filter.h delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.c delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.h diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 000000000..b70d3504e --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,28 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include "inertial_filter.h" + +void inertial_filter_predict(float dt, float x[3]) +{ + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +{ + float e = z - x[i]; + x[i] += e * w * dt; + + if (i == 0) { + x[1] += e * w * w * dt; + //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + + } else if (i == 1) { + x[2] += e * w * w * dt; + } +} diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h new file mode 100644 index 000000000..18c194abf --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -0,0 +1,13 @@ +/* + * inertial_filter.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include +#include + +void inertial_filter_predict(float dt, float x[3]); + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w); diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c deleted file mode 100644 index 390e1b720..000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * kalman_filter_inertial.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include "kalman_filter_inertial.h" - -void kalman_filter_inertial_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; -} - -void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { - float y[2]; - // y = z - H x - y[0] = z[0] - x[0]; - y[1] = z[1] - x[2]; - // x = x + K * y - for (int i = 0; i < 3; i++) { // Row - for (int j = 0; j < 2; j++) { // Column - if (use[j]) - x[i] += k[i][j] * y[j]; - } - } -} - -void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { - float y[2]; - // y = z - H x - y[0] = z[0] - x[0]; - y[1] = z[1] - x[1]; - y[2] = z[2] - x[2]; - // x = x + K * y - for (int i = 0; i < 3; i++) { // Row - for (int j = 0; j < 3; j++) { // Column - if (use[j]) - x[i] += k[i][j] * y[j]; - } - } -} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h deleted file mode 100644 index d34f58298..000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ /dev/null @@ -1,14 +0,0 @@ -/* - * kalman_filter_inertial.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include - -void kalman_filter_inertial_predict(float dt, float x[3]); - -void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); - -void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 8ac701c53..939d76849 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = position_estimator_inav SRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ - kalman_filter_inertial.c + inertial_filter.c diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6ecdfe01d..306ebe5cc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -66,7 +66,7 @@ #include #include "position_estimator_inav_params.h" -#include "kalman_filter_inertial.h" +#include "inertial_filter.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,13 +82,15 @@ static void usage(const char *reason); /** * Print the correct usage. */ -static void usage(const char *reason) { +static void usage(const char *reason) +{ if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); - } + + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + exit(1); +} /** * The position_estimator_inav_thread only briefly exists to start @@ -98,7 +100,8 @@ static void usage(const char *reason) { * The actual stack size should be set in the call * to task_create(). */ -int position_estimator_inav_main(int argc, char *argv[]) { +int position_estimator_inav_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -108,17 +111,19 @@ int position_estimator_inav_main(int argc, char *argv[]) { /* this is not an error */ exit(0); } + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, - position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL ); + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); } + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -127,9 +132,11 @@ int position_estimator_inav_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tposition_estimator_inav is running\n"); + } else { printf("\tposition_estimator_inav not started\n"); } + exit(0); } @@ -140,10 +147,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { /**************************************************************************** * main ****************************************************************************/ -int position_estimator_inav_thread_main(int argc, char *argv[]) { - /* welcome user */ - printf("[position_estimator_inav] started\n"); - static int mavlink_fd; +int position_estimator_inav_thread_main(int argc, char *argv[]) +{ + warnx("started."); + int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); @@ -156,9 +163,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0; //[°]] --> 47.0 - static double lon_current = 0.0; //[°]] -->8.5 - static double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0; //[°]] --> 47.0 + double lon_current = 0.0; //[°]] -->8.5 + double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -188,40 +195,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); - bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ + bool local_flag_baroINITdone = false; + /* first parameters read at start up */ struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ + /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - /* END FIRST PARAMETER UPDATE */ - /* wait until gps signal turns valid, only then can we initialize the projection */ + /* wait for GPS fix, only then can we initialize the projection */ if (params.use_gps) { struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (poll(fds_init, 1, 5000)) { if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } + static int printcounter = 0; + if (printcounter == 100) { printcounter = 0; - printf("[position_estimator_inav] wait for GPS fix type 3\n"); + warnx("waiting for GPS fix type 3..."); } + printcounter++; } /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; pos.home_lat = lat_current * 1e7; @@ -232,15 +241,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf( - "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", - lat_current, lon_current); - hrt_abstime last_time = 0; + warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + + hrt_abstime t_prev = 0; thread_running = true; - uint32_t accelerometer_counter = 0; + uint32_t accel_counter = 0; + hrt_abstime accel_t = 0; + float accel_dt = 0.0f; uint32_t baro_counter = 0; - uint16_t accelerometer_updates = 0; + hrt_abstime baro_t = 0; + hrt_abstime gps_t = 0; + uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; @@ -251,173 +263,184 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* main loop */ struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); + while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; bool gps_updated = false; - float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + hrt_abstime t = hrt_absolute_time(); + if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n"); + warnx("subscriptions poll error."); thread_should_exit = true; continue; + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); + &update); /* update parameters */ parameters_update(&pos_inav_param_handles, ¶ms); } + /* vehicle status */ if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + &vehicle_status); } + /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } + /* sensor combined */ if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accelerometer_counter) { + + if (sensor.accelerometer_counter > accel_counter) { accelerometer_updated = true; - accelerometer_counter = sensor.accelerometer_counter; - accelerometer_updates++; + accel_counter = sensor.accelerometer_counter; + accel_updates++; + accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; + accel_t = t; } + if (sensor.baro_counter > baro_counter) { baro_updated = true; baro_counter = sensor.baro_counter; baro_updates++; } - // barometric pressure estimation at start up + + /* barometric pressure estimation at start up */ if (!local_flag_baroINITdone && baro_updated) { - // mean calculation over several measurements + /* mean calculation over several measurements */ if (baro_loop_cnt < baro_loop_end) { baro_alt0 += sensor.baro_alt_meter; baro_loop_cnt++; + } else { - baro_alt0 /= (float) (baro_loop_cnt); + baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - char str[80]; - sprintf(str, - "[position_estimator_inav] baro_alt0 = %.2f", - baro_alt0); - printf("%s\n", str); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); } } } + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + /* project GPS lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double)(gps.lat)) * 1e-7, + ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), + &(proj_pos_gps[1])); + proj_pos_gps[2] = (float)(gps.alt * 1e-3); gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { pos.valid = true; } - } /* end of poll return value check */ + } + + /* end of poll return value check */ + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + t_prev = t; - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; if (att.R_valid) { /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter for altitude */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); + + /* inertial filter correction for altitude */ if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + if (baro_t > 0) { + inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); + } + + baro_t = t; } + if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); + inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); } if (params.use_gps) { - /* kalman filter for position */ - kalman_filter_inertial_predict(dt, x_est); - kalman_filter_inertial_predict(dt, y_est); - /* prepare vectors for kalman filter correction */ - float x_meas[3]; // position, velocity, acceleration - float y_meas[3]; // position, velocity, acceleration - bool use_xy[3] = { false, false, false }; + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); + + /* inertial filter correction for position */ if (gps_updated) { - x_meas[0] = local_pos_gps[0]; - y_meas[0] = local_pos_gps[1]; - x_meas[1] = gps.vel_n_m_s; - y_meas[1] = gps.vel_e_m_s; - use_xy[0] = true; - use_xy[1] = true; + if (gps_t > 0) { + float gps_dt = (t - gps_t) / 1000000.0f; + inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); + inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); + } + + gps_t = t; } + if (accelerometer_updated) { - x_meas[2] = accel_NED[0]; - y_meas[2] = accel_NED[1]; - use_xy[2] = true; - } - if (use_xy[0] || use_xy[2]) { - /* correction */ - kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); + inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); + inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); } } } + if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", - accelerometer_updates / updates_dt, - baro_updates / updates_dt, - gps_updates / updates_dt, - attitude_updates / updates_dt); + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + accel_updates / updates_dt, + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; - accelerometer_updates = 0; + accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; } } + if (t - pub_last > pub_interval) { pub_last = t; pos.x = x_est[0]; @@ -427,17 +450,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } - printf("[position_estimator_inav] exiting.\n"); + warnx("exiting."); mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 20142b70c..8dd0ceff8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -34,73 +34,39 @@ /* * @file position_estimator_inav_params.c - * + * * Parameters for position_estimator_inav */ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 0); - -PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_INT32(INAV_USE_GPS, 1); +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); - -int parameters_init(struct position_estimator_inav_param_handles *h) { +int parameters_init(struct position_estimator_inav_param_handles *h) +{ h->use_gps = param_find("INAV_USE_GPS"); - - h->k_alt_00 = param_find("INAV_K_ALT_00"); - h->k_alt_01 = param_find("INAV_K_ALT_01"); - h->k_alt_10 = param_find("INAV_K_ALT_10"); - h->k_alt_11 = param_find("INAV_K_ALT_11"); - h->k_alt_20 = param_find("INAV_K_ALT_20"); - h->k_alt_21 = param_find("INAV_K_ALT_21"); - - h->k_pos_00 = param_find("INAV_K_POS_00"); - h->k_pos_01 = param_find("INAV_K_POS_01"); - h->k_pos_02 = param_find("INAV_K_POS_02"); - h->k_pos_10 = param_find("INAV_K_POS_10"); - h->k_pos_11 = param_find("INAV_K_POS_11"); - h->k_pos_12 = param_find("INAV_K_POS_12"); - h->k_pos_20 = param_find("INAV_K_POS_20"); - h->k_pos_21 = param_find("INAV_K_POS_21"); - h->k_pos_22 = param_find("INAV_K_POS_22"); + h->w_alt_baro = param_find("INAV_W_ALT_BARO"); + h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); + h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); + h->w_pos_acc = param_find("INAV_W_POS_ACC"); return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ param_get(h->use_gps, &(p->use_gps)); - - param_get(h->k_alt_00, &(p->k_alt[0][0])); - param_get(h->k_alt_01, &(p->k_alt[0][1])); - param_get(h->k_alt_10, &(p->k_alt[1][0])); - param_get(h->k_alt_11, &(p->k_alt[1][1])); - param_get(h->k_alt_20, &(p->k_alt[2][0])); - param_get(h->k_alt_21, &(p->k_alt[2][1])); - - param_get(h->k_pos_00, &(p->k_pos[0][0])); - param_get(h->k_pos_01, &(p->k_pos[0][1])); - param_get(h->k_pos_02, &(p->k_pos[0][2])); - param_get(h->k_pos_10, &(p->k_pos[1][0])); - param_get(h->k_pos_11, &(p->k_pos[1][1])); - param_get(h->k_pos_12, &(p->k_pos[1][2])); - param_get(h->k_pos_20, &(p->k_pos[2][0])); - param_get(h->k_pos_21, &(p->k_pos[2][1])); - param_get(h->k_pos_22, &(p->k_pos[2][2])); + param_get(h->w_alt_baro, &(p->w_alt_baro)); + param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); + param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); + param_get(h->w_pos_acc, &(p->w_pos_acc)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index c0e0042b6..870227fef 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -34,7 +34,7 @@ /* * @file position_estimator_inav_params.h - * + * * Parameters for Position Estimator */ @@ -42,29 +42,20 @@ struct position_estimator_inav_params { int use_gps; - float k_alt[3][2]; - float k_pos[3][3]; + float w_alt_baro; + float w_alt_acc; + float w_pos_gps_p; + float w_pos_gps_v; + float w_pos_acc; }; struct position_estimator_inav_param_handles { param_t use_gps; - - param_t k_alt_00; - param_t k_alt_01; - param_t k_alt_10; - param_t k_alt_11; - param_t k_alt_20; - param_t k_alt_21; - - param_t k_pos_00; - param_t k_pos_01; - param_t k_pos_02; - param_t k_pos_10; - param_t k_pos_11; - param_t k_pos_12; - param_t k_pos_20; - param_t k_pos_21; - param_t k_pos_22; + param_t w_alt_baro; + param_t w_alt_acc; + param_t w_pos_gps_p; + param_t w_pos_gps_v; + param_t w_pos_acc; }; /** -- cgit v1.2.3 From eb76d116cc67c6354c29fa41e49b4cf9df1a472d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:30:42 +0200 Subject: Minor state machine improvements and fixes for IO safety / in-air restart handling --- src/drivers/px4io/px4io.cpp | 70 +++++++++++++++++++++------- src/modules/commander/state_machine_helper.c | 3 ++ src/modules/mavlink/orb_listener.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 11 +++-- src/modules/px4iofirmware/protocol.h | 5 +- src/modules/px4iofirmware/registers.c | 13 +++--- src/modules/px4iofirmware/safety.c | 10 ++-- 7 files changed, 78 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc65c4f25..f033382a6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -336,6 +336,7 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_safety(0), _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), @@ -389,6 +390,30 @@ PX4IO::init() */ _retries = 2; + /* get IO's last seen FMU state */ + int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + if (val == _io_reg_get_error) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE"); + } + uint16_t arming = val; + + /* get basic software version */ + /* basic configuration */ + usleep(5000); + + unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); + + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_PROTOCOL_VERSION); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + return 1; + } + /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); @@ -414,21 +439,23 @@ PX4IO::init() * in this case. */ - uint16_t reg; + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); - /* get IO's last seen FMU state */ - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); - if (ret != OK) - return ret; /* * in-air restart is only tried if the IO board reports it is * already armed, and has been configured for in-air restart */ - if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -482,7 +509,7 @@ PX4IO::init() cmd.confirmation = 1; /* send command once */ - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); /* spin here until IO's state has propagated into the system */ do { @@ -492,16 +519,22 @@ PX4IO::init() orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); } - /* wait 10 ms */ - usleep(10000); + /* wait 50 ms */ + usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } - /* keep waiting for state change for 10 s */ + /* re-send if necessary */ + if (!status.flag_system_armed) { + orb_publish(ORB_ID(vehicle_command), pub, &cmd); + log("re-sending arm cmd"); + } + + /* keep waiting for state change for 2 s */ } while (!status.flag_system_armed); /* regular boot, no in-air restart, init IO */ @@ -540,7 +573,7 @@ PX4IO::init() return -errno; } - mavlink_log_info(_mavlink_fd, "[IO] init ok"); + mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version); return OK; } @@ -863,14 +896,14 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_ARMED; + _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -891,7 +924,7 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.status = SAFETY_STATUS_UNLOCKED; } else { safety.status = SAFETY_STATUS_SAFE; @@ -1295,7 +1328,8 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0a6cfd0b5..f42caad57 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -545,6 +545,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* play warning tune */ tune_error(); + + /* abort */ + return; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 295cd5e28..9b2d984f0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -659,7 +659,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 123eb6778..b654bdec4 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -194,7 +194,7 @@ mixer_tick(void) */ bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) @@ -204,11 +204,15 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> armed"); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> disarmed"); } if (mixer_servos_armed) { @@ -263,9 +267,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..497e6af8e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -77,7 +77,7 @@ /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ @@ -93,7 +93,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..a092fc93b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -317,9 +317,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } r_setup_arming = value; @@ -367,9 +369,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a RC config change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..95335f038 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; -- cgit v1.2.3 From ec08dec8bae403f463ebf9e9a7b71b399ed7b97a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Jun 2013 12:47:00 +0200 Subject: Two hacks here to make it compile --- src/modules/gpio_led/gpio_led.c | 7 ++++--- src/modules/sdlog2/sdlog2.c | 20 +++++++++++++------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c7..542821e95 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -171,7 +171,8 @@ void gpio_led_cycle(FAR void *arg) /* select pattern for current status */ int pattern = 0; - if (priv->status.flag_system_armed) { + /* XXX fmu armed correct? */ + if (priv->status.flag_fmu_armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -180,10 +181,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (priv->status.arming_state == ARMING_STATE_STANDBY) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + } else if (priv->status.arming_state == ARMING_STATE_STANDBY && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80..8c5ec092d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -861,11 +861,16 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + // XXX fix this + // log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + // log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + // log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + // log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.state = 0; + log_msg.body.log_STAT.flight_mode = 0; + log_msg.body.log_STAT.manual_control_mode = 0; + log_msg.body.log_STAT.manual_sas_mode = 0; + log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1165,8 +1170,9 @@ void handle_command(struct vehicle_command_s *cmd) void handle_status(struct vehicle_status_s *status) { - if (status->flag_system_armed != flag_system_armed) { - flag_system_armed = status->flag_system_armed; + /* XXX fmu armed correct? */ + if (status->flag_fmu_armed != flag_system_armed) { + flag_system_armed = status->flag_fmu_armed; if (flag_system_armed) { sdlog2_start_log(); -- cgit v1.2.3 From c3a8f177b6316a9cefd814e312742f47d3049739 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:58:17 +0200 Subject: Software version check fixes --- src/drivers/px4io/px4io.cpp | 16 +++++++++++++--- src/modules/px4iofirmware/protocol.h | 7 +++++-- src/modules/px4iofirmware/registers.c | 4 ++-- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f033382a6..925041e0c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -404,13 +404,23 @@ PX4IO::init() unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); - if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) { mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", proto_version, - PX4IO_P_CONFIG_PROTOCOL_VERSION); + PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); - log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); + return 1; + } + + if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 497e6af8e..6c6c7b4e2 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -75,10 +75,13 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2 +#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a092fc93b..148514455 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC, + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, -- cgit v1.2.3 From 4860c73008c0bdfe69503fbbfa4e717a144fc3e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 06:48:24 +0400 Subject: multirotor_pos_control: position controller implemented --- .../multirotor_pos_control.c | 128 ++++++++++++++++----- .../multirotor_pos_control_params.c | 22 +++- .../multirotor_pos_control_params.h | 10 ++ 3 files changed, 126 insertions(+), 34 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6fe129d84..ad5670edc 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -59,7 +60,6 @@ #include #include #include -#include #include #include @@ -82,6 +82,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +static float scale_control(float ctl, float end, float dz); + +static float limit_value(float v, float limit); + +static float norm(float x, float y); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -137,9 +143,32 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } +static float scale_control(float ctl, float end, float dz) { + if (ctl > dz) { + return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + } else { + return 0.0f; + } +} + +static float limit_value(float v, float limit) { + if (v > limit) { + v = limit; + } else if (v < -limit) { + v = -limit; + } + return v; +} + +static float norm(float x, float y) { + return sqrtf(x * x + y * y); +} + static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor_pos_control] started\n"); + warnx("started."); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -175,7 +204,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_pos = true; hrt_abstime t_prev = 0; float alt_integral = 0.0f; - float alt_ctl_dz = 0.2f; + /* integrate in NED frame to estimate wind but not attitude offset */ + float pos_x_integral = 0.0f; + float pos_y_integral = 0.0f; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; thread_running = true; @@ -235,64 +268,97 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - char str[80]; - sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, str); + pos_x_integral = 0.0f; + pos_y_integral = 0.0f; + mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ - float alt_ctl = manual.throttle - 0.5f; - if (fabs(alt_ctl) < alt_ctl_dz) { - alt_ctl = 0.0f; - } else { - if (alt_ctl > 0.0f) - alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); - else - alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); - local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; - if (local_pos_sp.z > local_pos.z + err_linear_limit) - local_pos_sp.z = local_pos.z + err_linear_limit; - else if (local_pos_sp.z < local_pos.z - err_linear_limit) - local_pos_sp.z = local_pos.z - err_linear_limit; + float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (alt_sp_ctl != 0.0f) { + local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { + local_pos_sp.z = local_pos.z + alt_err_linear_limit; + } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { + local_pos_sp.z = local_pos.z - alt_err_linear_limit; + } } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO move position setpoint with manual controls + /* move position setpoint with manual controls */ + float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { + /* calculate direction and increment of control in NED frame */ + float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; + local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + /* limit maximum setpoint from position offset and preserve direction */ + float pos_vec_x = local_pos_sp.x - local_pos.x; + float pos_vec_y = local_pos_sp.y - local_pos.y; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + if (pos_vec_norm > 1.0f) { + local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; + local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; + } + } } } /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } + float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); /* P and D components */ float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; /* add I component */ float thrust_ctl = thrust_ctl_pd + alt_integral; + /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (alt_integral < params.thr_min) { + alt_integral = params.thr_min; + } else if (alt_integral > params.thr_max) { + alt_integral = params.thr_max; + } if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { thrust_ctl = params.thr_max; } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - // TODO add position controller + /* PID for position */ + /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ + float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); + float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); + /* P and D components */ + float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + /* integrate */ + pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* calculate direction and slope in NED frame */ + float dir = atan2f(pos_y_ctl, pos_x_ctl); + /* use approximation: slope ~ sin(slope) = force */ + float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); + /* convert direction to body frame */ + dir -= att.yaw; + /* calculate roll and pitch */ + att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch + att_sp.roll_body = sinf(dir) * slope; } else { reset_sp_pos = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 90dd820f4..9610ef9f7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -45,19 +45,29 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); int parameters_init(struct multirotor_position_control_param_handles *h) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - /* PID parameters */ h->alt_p = param_find("MPC_ALT_P"); h->alt_i = param_find("MPC_ALT_I"); h->alt_d = param_find("MPC_ALT_D"); h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); + h->pos_p = param_find("MPC_POS_P"); + h->pos_i = param_find("MPC_POS_I"); + h->pos_d = param_find("MPC_POS_D"); + h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); + h->slope_max = param_find("MPC_SLOPE_MAX"); + return OK; } @@ -69,5 +79,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->alt_i, &(p->alt_i)); param_get(h->alt_d, &(p->alt_d)); param_get(h->alt_rate_max, &(p->alt_rate_max)); + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_i, &(p->pos_i)); + param_get(h->pos_d, &(p->pos_d)); + param_get(h->pos_rate_max, &(p->pos_rate_max)); + param_get(h->slope_max, &(p->slope_max)); + return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 849055cd1..589ee92a1 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -48,6 +48,11 @@ struct multirotor_position_control_params { float alt_i; float alt_d; float alt_rate_max; + float pos_p; + float pos_i; + float pos_d; + float pos_rate_max; + float slope_max; }; struct multirotor_position_control_param_handles { @@ -57,6 +62,11 @@ struct multirotor_position_control_param_handles { param_t alt_i; param_t alt_d; param_t alt_rate_max; + param_t pos_p; + param_t pos_i; + param_t pos_d; + param_t pos_rate_max; + param_t slope_max; }; /** -- cgit v1.2.3 From 4cdee2be03b693b843c4dec93e67eadec903f5d8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 06:49:17 +0400 Subject: position_estimator_inav cosmetic changes --- .../position_estimator_inav_main.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 306ebe5cc..07ea7cd5c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -152,7 +152,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("started."); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + mavlink_log_info(mavlink_fd, "[inav] started"); /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -241,8 +241,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - - warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current); hrt_abstime t_prev = 0; thread_running = true; @@ -269,7 +269,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - printf("[position_estimator_inav] main loop started\n"); + warnx("main loop started."); while (!thread_should_exit) { bool accelerometer_updated = false; @@ -337,7 +337,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); + warnx("baro_alt0 = %.2f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0); + pos.home_alt = baro_alt0; } } } @@ -428,7 +430,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", accel_updates / updates_dt, baro_updates / updates_dt, gps_updates / updates_dt, @@ -462,7 +464,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } warnx("exiting."); - mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); + mavlink_log_info(mavlink_fd, "[inav] exiting"); thread_running = false; return 0; } -- cgit v1.2.3 From 236053a600f5708aee0e5849f4fefc2380e7d101 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Jun 2013 15:04:16 +0200 Subject: Fixed param save --- src/modules/commander/commander.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index a34e526a8..c2a7242d1 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -955,7 +955,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* check if no other task is scheduled */ if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; -- cgit v1.2.3 From e4b25f857016302fa254d41a964e586da49dd4b3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 17:12:13 +0400 Subject: Default parameters updated for position_estimator_inav and multirotor_pos_control --- src/modules/multirotor_pos_control/multirotor_pos_control_params.c | 4 ++-- .../position_estimator_inav/position_estimator_inav_params.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 9610ef9f7..7f2ba3db8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -51,8 +51,8 @@ PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); -PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 8dd0ceff8..49dc7f51f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,9 +43,9 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { -- cgit v1.2.3 From 53f29a25b6c011d4cf4992a9eb1207a344ee75ae Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Thu, 13 Jun 2013 12:51:50 -0700 Subject: Added l3gd20h detection --- src/drivers/l3gd20/l3gd20.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 98098c83b..f47481823 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -78,6 +78,7 @@ static const int ERROR = -1; /* register addresses */ #define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 #define ADDR_CTRL_REG1 0x20 @@ -351,7 +352,7 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) return OK; return -EIO; -- cgit v1.2.3 From 90f5e30f2a177bed2ac08e76699ec3029292d640 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 13:53:26 +0200 Subject: Introduced new actuator_safety topic --- src/drivers/ardrone_interface/ardrone_interface.c | 15 +- src/drivers/blinkm/blinkm.cpp | 21 ++- src/drivers/hil/hil.cpp | 17 ++- src/drivers/mkblctrl/mkblctrl.cpp | 19 +-- src/drivers/px4fmu/fmu.cpp | 27 ++-- src/drivers/px4io/px4io.cpp | 41 ++--- src/modules/commander/commander.c | 170 ++++++++++++--------- src/modules/commander/state_machine_helper.c | 25 +-- src/modules/commander/state_machine_helper.h | 3 +- src/modules/gpio_led/gpio_led.c | 16 +- src/modules/mavlink/orb_listener.c | 18 +-- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 8 +- src/modules/mavlink_onboard/orb_topics.h | 3 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 21 ++- src/modules/sdlog2/sdlog2.c | 23 ++- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/actuator_controls.h | 15 +- src/modules/uORB/topics/actuator_safety.h | 65 ++++++++ src/modules/uORB/topics/vehicle_control_mode.h | 96 ++++++++++++ src/modules/uORB/topics/vehicle_status.h | 5 +- 22 files changed, 424 insertions(+), 193 deletions(-) create mode 100644 src/modules/uORB/topics/actuator_safety.h create mode 100644 src/modules/uORB/topics/vehicle_control_mode.h diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 264041e65..4a6f30a4f 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_armed_s armed; - armed.armed = false; + struct actuator_safety_s safety; + safety.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -322,18 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } else { /* MAIN OPERATION MODE */ - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* for now only spin if armed and immediately shut down * if in failsafe */ - //XXX fix this - //if (armed.armed && !armed.lockdown) { - if (state.flag_fmu_armed) { + if (safety.armed && !safety.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 7a64b72a4..1cfdcfbed 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,7 @@ #include #include #include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,6 +377,7 @@ BlinkM::led() static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +388,7 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_actuator_safety = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +401,9 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +458,14 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct actuator_safety_s actuator_safety; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_actuator_safety; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +479,17 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + + if (new_data_actuator_safety) { + orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); + no_data_actuator_safety = 0; + } else { + no_data_actuator_safety++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -530,7 +549,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_fmu_armed == false) { + if(actuator_safety.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d9aa772d4..e851d8a52 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #include @@ -108,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_safety; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_safety(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -321,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -334,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_safety; fds[1].events = POLLIN; unsigned num_outputs; @@ -426,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); } } ::close(_t_actuators); - ::close(_t_armed); + ::close(_t_safety); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8a..093b53d42 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -132,7 +133,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; unsigned int _motor; int _px4mode; int _frametype; @@ -240,7 +241,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -496,8 +497,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -519,7 +520,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; log("starting"); @@ -625,13 +626,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(as.armed && !as.lockdown); } @@ -639,7 +640,7 @@ MK::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf72892eb..db4c87ddc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -174,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -376,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -396,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -499,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = as.armed && !as.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -535,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1021,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_armed_s aa; + actuator_safety_s as; - aa.armed = true; - aa.lockdown = false; + as.armed = true; + as.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_armed), &aa); + handle = orb_advertise(ORB_ID(actuator_safety), &as); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 962a91c7f..28a3eb917 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -152,7 +153,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic + int _t_actuator_safety; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -327,7 +328,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -433,20 +434,20 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_safety_s armed; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - /* keep checking for an update, ensure we got a recent state, + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); break; } @@ -472,10 +473,10 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - cmd.source_system = status.system_id; - cmd.source_component = status.component_id; + // cmd.target_system = status.system_id; + // cmd.target_component = status.component_id; + // cmd.source_system = status.system_id; + // cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; @@ -484,10 +485,10 @@ PX4IO::init() /* spin here until IO's state has propagated into the system */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); } /* wait 10 ms */ @@ -500,7 +501,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_fmu_armed); + } while (!armed.armed); /* regular boot, no in-air restart, init IO */ } else { @@ -564,8 +565,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -577,7 +578,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -713,16 +714,16 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_armed_s armed; ///< system armed state + actuator_safety_s safety; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (safety.armed && !safety.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c2a7242d1..6812fb1fb 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -73,8 +73,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -137,10 +139,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* Main state machine */ -static struct vehicle_status_s current_status; -static orb_advert_t stat_pub; - /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; @@ -187,7 +185,7 @@ void do_rc_calibration(void); void do_airspeed_calibration(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -304,10 +302,11 @@ void do_rc_calibration() { mavlink_log_info(mavlink_fd, "trim calibration starting"); - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; @@ -718,7 +717,7 @@ void do_airspeed_calibration() close(diff_pres_sub); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -738,13 +737,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -757,14 +756,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -777,7 +776,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -826,7 +825,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -845,7 +844,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -865,7 +864,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -886,7 +885,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -906,7 +905,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -925,7 +924,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -1189,17 +1188,30 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + /* safety topic */ + struct actuator_safety_s safety; + orb_advert_t safety_pub; + /* Initialize safety with all false */ + memset(&safety, 0, sizeof(safety)); + + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_hil_enabled = false; - current_status.flag_fmu_armed = false; - current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1222,16 +1234,18 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; memset(&home, 0, sizeof(home)); - if (stat_pub < 0) { + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); exit(ERROR); @@ -1333,6 +1347,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1386,9 +1401,11 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); + handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); } + + /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -1397,9 +1414,8 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ - if (!current_status.flag_fmu_armed) { + if (!safety.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1417,7 +1433,6 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(current_status.system_id)); param_get(_param_component_id, &(current_status.component_id)); - } } @@ -1564,7 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; // XXX implement this - // state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1579,7 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1694,7 +1709,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_fmu_armed) { + && !safety.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1734,24 +1749,22 @@ int commander_thread_main(int argc, char *argv[]) warnx("mode sw not finite"); /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; - printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; - printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; - printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1811,7 +1824,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1819,9 +1832,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1830,11 +1843,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1850,7 +1863,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1859,9 +1872,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1871,9 +1884,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1885,19 +1898,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1909,10 +1922,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1924,10 +1937,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1975,13 +1988,22 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - (sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + // printf("checking\n"); + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + + + printf("System Type: %d\n", current_status.system_type); + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + + } else { + mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + } stick_off_counter = 0; } else { @@ -1993,7 +2015,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; } else { @@ -2095,13 +2117,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_fmu_armed) { + if (sp_offboard.armed && !safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { + } else if (!sp_offboard.armed && safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } } else { @@ -2143,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_preflight_mag_calibration == false && // current_status.flag_preflight_accel_calibration == false) { // /* All ok, no calibration going on, go to standby */ - // do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); // } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); state_changed = false; } diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index daed81553..fea7ee840 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -54,7 +54,7 @@ #include "state_machine_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { int ret = ERROR; @@ -69,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_STANDBY: @@ -81,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -95,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -105,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -114,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_REBOOT: @@ -125,7 +125,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; @@ -139,7 +139,12 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (ret == OK) { current_state->arming_state = new_arming_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + safety->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_safety), safety_pub, safety); } } @@ -460,7 +465,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (ret == OK) { current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 5b57cffb7..824a7529f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,6 +46,7 @@ #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -53,7 +54,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 542821e95..618095d0b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -60,6 +61,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_safety_s safety; + int actuator_safety_sub; bool led_state; int counter; }; @@ -168,11 +171,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + /* select pattern for current status */ int pattern = 0; - /* XXX fmu armed correct? */ - if (priv->status.flag_fmu_armed) { + if (priv->safety.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -181,11 +188,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.arming_state == ARMING_STATE_STANDBY) { + if (priv->safety.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.arming_state == ARMING_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d84406e7a..a4a2ca3e5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_armed_s armed; +struct actuator_safety_s safety; struct mavlink_subscriptions mavlink_subs; @@ -109,7 +109,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); +static void l_actuator_safety(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -133,7 +133,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_actuator_safety, &mavlink_subs.safety_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && safety.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +l_actuator_safety(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); } void @@ -745,8 +745,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index d61cd43dc..e647b090a 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -72,7 +73,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 408a850d8..4b6d81113 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,7 +274,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_fmu_armed) { + if (safety->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -369,7 +369,7 @@ int mavlink_thread_main(int argc, char *argv[]) baudrate = 57600; struct vehicle_status_s v_status; - struct actuator_armed_s armed; + struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -437,7 +437,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 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); + get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f56243..fee5580b3 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -69,7 +70,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372..17c3ba820 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,5 @@ 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, +extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 3329c5c48..44c2fb1d8 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -84,13 +85,16 @@ static bool motor_test_mode = false; static orb_advert_t actuator_pub; -static struct vehicle_status_s state; + static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ + struct vehicle_status_s state; memset(&state, 0, sizeof(state)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -113,6 +117,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -150,7 +155,7 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_fmu_armed = false; + bool flag_armed = false; bool flag_control_position_enabled = false; bool flag_control_velocity_enabled = false; @@ -200,6 +205,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &state); } + orb_check(safety_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -248,7 +259,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_fmu_armed) { + safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -292,7 +303,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_fmu_armed) { + if (!flag_control_attitude_enabled && safety.armed) { att_sp.yaw_body = att.yaw; } @@ -399,7 +410,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = state.flag_control_manual_enabled; flag_control_position_enabled = state.flag_control_position_enabled; flag_control_velocity_enabled = state.flag_control_velocity_enabled; - flag_fmu_armed = state.flag_fmu_armed; + flag_armed = safety.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8c5ec092d..41c2f67e5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -190,7 +191,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct vehicle_status_s *cmd); +static void handle_status(struct actuator_safety_s *safety); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -623,6 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { + struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -643,6 +645,7 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { + int safety_sub; int cmd_sub; int status_sub; int sensor_sub; @@ -690,6 +693,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- SAFETY STATUS --- */ + subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + fds[fdsc_count].fd = subs.safety_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; @@ -826,6 +835,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -838,7 +848,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf_status); + handle_status(&buf.safety); } handled_topics++; @@ -870,7 +880,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1168,11 +1178,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct vehicle_status_s *status) +void handle_status(struct actuator_safety_s *safety) { - /* XXX fmu armed correct? */ - if (status->flag_fmu_armed != flag_system_armed) { - flag_system_armed = status->flag_fmu_armed; + if (safety->armed != flag_system_armed) { + flag_system_armed = safety->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2..2674354c3 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -137,7 +137,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_safety.h" +ORB_DEFINE(actuator_safety, struct actuator_safety_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..26b967237 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; @@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - #endif \ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h new file mode 100644 index 000000000..b78eb4b7e --- /dev/null +++ b/src/modules/uORB/topics/actuator_safety.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_SAFETY_H +#define TOPIC_ACTUATOR_SAFETY_H + +#include +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_safety_s { + + uint64_t timestamp; + + bool safety_off; /**< Set to true if safety is off */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 000000000..eb35d0884 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes + * + * 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 vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ + bool flag_system_emergency; + bool flag_preflight_calibration; + + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_auto_enabled; + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + bool failsave_highlevel; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6d3f8a863..b19075921 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,8 +199,8 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ - bool flag_io_armed; /**< true is motors / actuators of IO are armed */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -245,7 +245,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - }; /** -- cgit v1.2.3 From 5b21362e1ffefe4e28579eb7a853fe5d22288760 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:04:23 +0200 Subject: Arming with IO working now --- src/drivers/px4io/px4io.cpp | 11 +++++------ src/modules/commander/state_machine_helper.c | 4 ++++ 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 28a3eb917..d728b7b76 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -728,12 +728,11 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - // TODO fix this -// if (armed.ready_to_arm) { -// set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } else { -// clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } + if (safety.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index fea7ee840..4f2fbc984 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,6 +70,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -82,6 +83,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->condition_system_sensors_initialized) { ret = OK; safety->armed = false; + safety->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -115,6 +117,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -126,6 +129,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; -- cgit v1.2.3 From e556649f2ff6922a7a3b7751b68cdedd0d6254aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:48:41 +0200 Subject: Beep when mode is not possible --- src/modules/commander/commander.c | 3 --- src/modules/commander/state_machine_helper.c | 19 +++++++++++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 6812fb1fb..1d3f90807 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1991,9 +1991,6 @@ int commander_thread_main(int argc, char *argv[]) // printf("checking\n"); if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - - printf("System Type: %d\n", current_status.system_type); if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 4f2fbc984..dcaf775b9 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -198,6 +198,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed first */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed first */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -236,8 +238,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -265,8 +269,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -293,8 +299,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -317,10 +325,13 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -343,6 +354,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -380,8 +392,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -405,6 +419,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a mission ready */ if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -428,8 +443,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -450,8 +467,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; -- cgit v1.2.3 From 38ca3bd78a9b415df4a260a8cba43e2c13703972 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 15 Jun 2013 11:36:26 +0400 Subject: multirotor_pos_control fixes, introduced HARD control mode (disabled by default) --- .../multirotor_pos_control.c | 43 ++++++++++++++-------- .../multirotor_pos_control_params.c | 11 ++++++ .../multirotor_pos_control_params.h | 10 +++++ 3 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index ad5670edc..c94972e7d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -283,11 +283,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float pos_sp_speed_x = 0.0f; + float pos_sp_speed_y = 0.0f; + float pos_sp_speed_z = 0.0f; + if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { - local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; + local_pos_sp.z += pos_sp_speed_z * dt; if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { local_pos_sp.z = local_pos.z + alt_err_linear_limit; } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { @@ -297,14 +302,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; - local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; - local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; + pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.x += pos_sp_speed_x * dt; + local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; @@ -315,15 +322,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } } } + + if (params.hard == 0) { + pos_sp_speed_x = 0.0f; + pos_sp_speed_y = 0.0f; + pos_sp_speed_z = 0.0f; + } } /* PID for altitude */ /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); + float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); /* P and D components */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; + float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; if (alt_integral < params.thr_min) { @@ -331,6 +342,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } else if (alt_integral > params.thr_max) { alt_integral = params.thr_max; } + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { @@ -342,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; /* integrate */ pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; /* calculate direction and slope in NED frame */ float dir = atan2f(pos_y_ctl, pos_x_ctl); /* use approximation: slope ~ sin(slope) = force */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7f2ba3db8..7c3296cae 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -53,6 +53,7 @@ PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -67,6 +68,11 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); + h->slope_max = param_find("MPC_HARD"); + + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } @@ -84,6 +90,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->pos_d, &(p->pos_d)); param_get(h->pos_rate_max, &(p->pos_rate_max)); param_get(h->slope_max, &(p->slope_max)); + param_get(h->hard, &(p->hard)); + + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 589ee92a1..13b667ad3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -53,6 +53,11 @@ struct multirotor_position_control_params { float pos_d; float pos_rate_max; float slope_max; + int hard; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { @@ -67,6 +72,11 @@ struct multirotor_position_control_param_handles { param_t pos_d; param_t pos_rate_max; param_t slope_max; + param_t hard; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** -- cgit v1.2.3 From 9f5565de3221718ba12800a54ca1a0c06b7491ef Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Jun 2013 19:41:54 +0200 Subject: Controllers should not access state machine anymore but access the vehicle_control_mode flags --- src/modules/commander/commander.c | 117 +++++++++--------- src/modules/commander/state_machine_helper.c | 136 +++++++++++---------- src/modules/commander/state_machine_helper.h | 4 +- src/modules/mavlink/orb_topics.h | 1 + src/modules/mavlink_onboard/mavlink.c | 12 +- src/modules/mavlink_onboard/orb_topics.h | 1 + src/modules/mavlink_onboard/util.h | 3 +- .../multirotor_att_control_main.c | 45 +++---- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/actuator_safety.h | 1 + src/modules/uORB/topics/vehicle_control_mode.h | 9 +- src/modules/uORB/topics/vehicle_status.h | 18 +-- 12 files changed, 182 insertions(+), 168 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 1d3f90807..7853d2e79 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1233,6 +1233,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized current_status.condition_system_sensors_initialized = true; + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1240,6 +1243,8 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -1824,7 +1829,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1832,9 +1837,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1843,11 +1848,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1863,7 +1868,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1872,9 +1877,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1884,9 +1889,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1898,19 +1903,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1922,10 +1927,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1937,10 +1942,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -2070,43 +2075,43 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } - - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_positive(); + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_positive(); - } - } + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); + + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index dcaf775b9..394ee67cc 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -161,7 +162,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { int ret = ERROR; @@ -179,11 +180,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; - current_state->flag_control_rates_enabled = false; - current_state->flag_control_attitude_enabled = false; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } break; @@ -201,11 +202,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } } break; @@ -218,11 +219,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } break; @@ -244,11 +245,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -275,11 +276,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -305,11 +306,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -334,11 +335,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -357,11 +358,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -372,11 +373,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } break; @@ -398,11 +399,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -422,11 +423,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -449,11 +450,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -473,11 +474,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -491,6 +492,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->counter++; current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 824a7529f..8d8536090 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -54,8 +55,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index e647b090a..221957d46 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 4b6d81113..dbea2be08 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,18 +274,18 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, 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) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (safety->hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -296,7 +296,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status->flag_control_velocity_enabled) { + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; @@ -368,7 +368,9 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ @@ -437,7 +439,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 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, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index fee5580b3..f01f09e12 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 17c3ba820..c6a2e52bf 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ 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_safety_s *safety, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 44c2fb1d8..b4d7fb630 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -91,8 +91,8 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct actuator_safety_s safety; memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; @@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -181,7 +181,6 @@ mc_thread_main(int argc, char *argv[]) } else if (ret == 0) { /* no return value, ignore */ } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -199,10 +198,10 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; - orb_check(state_sub, &updated); + orb_check(control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } orb_check(safety_sub, &updated); @@ -227,9 +226,8 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; @@ -252,13 +250,11 @@ mc_thread_main(int argc, char *argv[]) } - } else if (state.flag_control_manual_enabled) { - - if (state.flag_control_attitude_enabled) { - + } else if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || + if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || + control_mode.flag_control_manual_enabled != flag_control_manual_enabled || safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -266,6 +262,8 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +/* XXX fix this */ +#if 0 if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); @@ -297,6 +295,7 @@ mc_thread_main(int argc, char *argv[]) rc_loss_first_time = false; } else { +#endif rc_loss_first_time = true; att_sp.roll_body = manual.roll; @@ -344,7 +343,9 @@ mc_thread_main(int argc, char *argv[]) att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); +#if 0 } +#endif /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -378,7 +379,7 @@ mc_thread_main(int argc, char *argv[]) } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { + if (control_mode.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -405,11 +406,11 @@ mc_thread_main(int argc, char *argv[]) multirotor_control_rates(&rates_sp, gyro, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_control_position_enabled = state.flag_control_position_enabled; - flag_control_velocity_enabled = state.flag_control_velocity_enabled; + /* update control_mode */ + flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; + flag_control_manual_enabled = control_mode.flag_control_manual_enabled; + flag_control_position_enabled = control_mode.flag_control_position_enabled; + flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; flag_armed = safety.armed; perf_end(mc_loop_perf); @@ -427,7 +428,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); - close(state_sub); + close(control_mode_sub); close(manual_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 2674354c3..313fbf641 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -98,6 +98,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index b78eb4b7e..3a107d41a 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -58,6 +58,7 @@ struct actuator_safety_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ }; ORB_DECLARE(actuator_safety); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index eb35d0884..177e30898 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,16 +61,11 @@ */ struct vehicle_control_mode_s { - /* use of a counter and timestamp recommended (but not necessary) */ - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -83,7 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; + bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ }; /** @@ -91,6 +86,6 @@ struct vehicle_control_mode_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_status); +ORB_DECLARE(vehicle_control_mode); #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b19075921..2bcd57f4b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,18 +199,18 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ + //bool flag_armed; /**< true is motors / actuators are armed */ + //bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ + // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + // bool flag_auto_enabled; + // bool flag_control_rates_enabled; /**< true if rates are stabilized */ + // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + // bool flag_control_position_enabled; /**< true if position is controlled */ // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ -- cgit v1.2.3 From 2b9fa731efd08a01effd87a636ae8e53994944f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Mar 2013 12:13:02 -0700 Subject: Use the pid library in the rate controller and change de implementation of the D part Conflicts: src/modules/multirotor_att_control/multirotor_rate_control.c src/modules/systemlib/pid/pid.c src/modules/systemlib/pid/pid.h --- .../multirotor_attitude_control.c | 47 +++------------------- .../multirotor_rate_control.c | 35 ++++++++++------ src/modules/systemlib/pid/pid.c | 28 ++++++++----- src/modules/systemlib/pid/pid.h | 3 +- 4 files changed, 46 insertions(+), 67 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 76dbb36d3..1053ac7a3 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,29 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - //float yaw_awu; - //float yaw_lim; float att_p; float att_i; float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; }; /** @@ -122,17 +101,10 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,17 +114,10 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); return OK; } @@ -170,8 +135,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s last_input = att_sp->timestamp; } - static int sensor_delay; - sensor_delay = hrt_absolute_time() - att->timestamp; +// static int sensor_delay; +// sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; @@ -190,10 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -205,7 +168,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index deba1ac03..4ab92b955 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -150,13 +150,11 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; +// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; @@ -166,9 +164,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; + float pitch_control_last = 0.0f; + float roll_control_last = 0.0f; + static bool initialized = false; /* initialize the pid controllers when the function is called for the first time */ @@ -176,39 +180,44 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", - // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); } - /* calculate current control outputs */ - /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT); + + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; } else { - pitch_control = 0.0f; + pitch_control = pitch_control_last; warnx("rej. NaN ctrl pitch"); } - /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; } else { - roll_control = 0.0f; + roll_control = roll_control_last; warnx("rej. NaN ctrl roll"); } diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9..08e0ca46f 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,13 +51,14 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->count = 0; - pid->saturated = 0; - pid->last_output = 0; - - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; + + pid->sp = 0.0f; + pid->error_previous_filtered = 0.0f; + pid->control_previous = 0.0f; + pid->integral = 0.0f; } __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { @@ -136,15 +137,15 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - if (isfinite(error)) { // Why is this necessary? DEW - pid->error_previous = error; - } + float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; +// d = (error_filtered - pid->error_previous_filtered) / dt; + d = pid->error_previous_filtered - error_filtered; + pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -180,6 +181,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->last_output = output; } + pid->control_previous = pid->last_output; + + // if (isfinite(error)) { // Why is this necessary? DEW + // pid->error_previous = error; + // } return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 64d668867..5790459c8 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -59,7 +59,8 @@ typedef struct { float intmax; float sp; float integral; - float error_previous; + float error_previous_filtered; + float control_previous; float last_output; float limit; uint8_t mode; -- cgit v1.2.3 From 8559315f4f8b6515f2ba9ceadf2aa3b73af41bdc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 10:53:20 -0700 Subject: Added a filter parameter to the pid function Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c --- .../fixedwing_att_control/fixedwing_att_control_att.c | 8 ++++---- .../fixedwing_att_control/fixedwing_att_control_rate.c | 12 ++++++------ .../fixedwing_pos_control/fixedwing_pos_control_main.c | 16 ++++++++-------- .../multirotor_att_control/multirotor_attitude_control.c | 8 ++++---- .../multirotor_att_control/multirotor_rate_control.c | 8 ++++---- src/modules/systemlib/pid/pid.c | 14 +++++++++++--- src/modules/systemlib/pid/pid.h | 5 +++-- 7 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 769b8b0a8..a226757e0 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -137,8 +137,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0); } /* Roll (P) */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 4eccc118c..79194e515 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -189,9 +189,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0); } diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 71c78f5b8..48ec3603f 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); @@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value } /* only run controller if attitude changed */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 1053ac7a3..d7e1a3adc 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 4ab92b955..57aea726a 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -182,17 +182,17 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, initialized = true; pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_CALC); + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_CALC); + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); } /* control pitch (forward) output */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 08e0ca46f..d0e67d3ea 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, float diff_filter_factor, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -51,6 +51,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; + pid->diff_filter_factor = diff_filter_factor; pid->count = 0.0f; pid->saturated = 0.0f; pid->last_output = 0.0f; @@ -60,7 +61,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->control_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) { int ret = 0; @@ -99,6 +100,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } + if (isfinite(diff_filter_factor)) { + pid->limit = diff_filter_factor; + + } else { + ret = 1; + } + return ret; } @@ -137,7 +145,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered; + float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; // Calculate or measured current error derivative diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 5790459c8..f89c36d75 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -64,12 +64,13 @@ typedef struct { float last_output; float limit; uint8_t mode; + float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); -- cgit v1.2.3 From 263b60c200b80af68fea7a491cb17e9db4f65135 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:54:57 +0200 Subject: Hack to make flow controll to compile --- src/examples/flow_position_control/flow_position_control_main.c | 4 +++- src/examples/flow_position_estimator/flow_position_estimator_main.c | 4 ++++ src/examples/flow_speed_control/flow_speed_control_main.c | 2 ++ 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index c177c8fd2..5246ea62b 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -268,6 +268,8 @@ flow_position_control_thread_main(int argc, char *argv[]) /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); +// XXX fix this +#if 0 if (vstatus.state_machine == SYSTEM_STATE_AUTO) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 @@ -527,7 +529,7 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } - +#endif /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c0b16d2e7..2389c693d 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -218,6 +218,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { +// XXX fix this +#if 0 if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -273,6 +275,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) * -> minimum sonar value 0.3m */ + if (!vehicle_liftoff) { if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) @@ -437,6 +440,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } counter++; + #endif } printf("[flow position estimator] exiting.\n"); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 9648728c8..0be4b3d5a 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -186,6 +186,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { +#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -340,6 +341,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } +#endif } printf("[flow speed control] ending now...\n"); -- cgit v1.2.3 From 68fb200f0bc7e995fd604ee9e345f37839d02ced Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: Fixed pid bug, attitude was not controlled --- .../multirotor_att_control/multirotor_attitude_control.c | 10 ++++++---- src/modules/systemlib/pid/pid.c | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adc..4c8f72b8d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea..0188a51c4 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { ret = 1; @@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; -- cgit v1.2.3 From 1ea9ff36402384d66879216a3bc7509651af0be6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- src/modules/fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- src/modules/fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- src/modules/multirotor_att_control/multirotor_attitude_control.c | 4 ++-- src/modules/multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0..7009356b7 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e515..991d5ec5d 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603f..9c19c6786 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d..51faaa8c0 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726a..322c5485a 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4..3685b2aeb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d75..e3d9038cd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); -- cgit v1.2.3 From 562253c508d1a758207d21e116cbbc194d7e0721 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 11:55:08 +0200 Subject: Fixed bug that I introduced in sdlog2 --- src/modules/sdlog2/sdlog2.c | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 41c2f67e5..940f30a46 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -613,7 +613,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 16; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -622,9 +622,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); + struct actuator_safety_s buf_safety; + memset(&buf_safety, 0, sizeof(buf_safety)); + /* warning! using union here to save memory, elements should be used separately! */ union { - struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -645,9 +647,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { - int safety_sub; int cmd_sub; int status_sub; + int safety_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -693,7 +695,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY STATUS --- */ + /* --- SAFETY --- */ subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); fds[fdsc_count].fd = subs.safety_sub; fds[fdsc_count].events = POLLIN; @@ -835,7 +837,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; - /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -843,22 +844,33 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + /* --- SAFETY- LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); if (log_when_armed) { - handle_status(&buf.safety); + handle_status(&buf_safety); } handled_topics++; } + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + + //if (log_when_armed) { + // handle_status(&buf_safety); + //} + + //handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // Begin from fds[1] again + ifds = 2; // Begin from fds[2] again pthread_mutex_lock(&logbuffer_mutex); @@ -880,7 +892,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; -- cgit v1.2.3 From b52d561b11298abb2982b786676f49eea96259d8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: Added ctrl debugging values --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 ++++++++- src/modules/sdlog2/sdlog2_messages.h | 33 +++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/vehicle_control_debug.h | 87 ++++++++++++++++++++++ 5 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 322c5485a..01bf383e2 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include #include #include +#include +#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + static struct vehicle_control_debug_s control_debug; + static orb_advert_t control_debug_pub; + + + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, NULL, NULL, NULL); + rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, NULL, NULL, NULL); + rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; + + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 940f30a46..844e02268 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -613,7 +614,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -639,6 +640,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -661,6 +663,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; } subs; @@ -680,6 +683,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; } body; @@ -773,6 +777,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -1058,6 +1068,30 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; + log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; + log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e..a80af33ef 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -156,14 +156,42 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + + float pitch_p; + float pitch_i; + float pitch_d; + + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; + + float yaw_p; + float yaw_i; + float yaw_d; + + float yaw_rate_p; + float yaw_rate_i; + float yaw_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; @@ -182,6 +210,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRateP,RollRateI,RollRateD,PitchP,PitchI,PitchD,PitchRateP,PitchRateI,PitchRateD,YawP,YawI,YawD,YawRateP,YawRateI,YawRateD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7dcb9cf93..f1f07441d 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -119,6 +119,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 000000000..6184284a4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif -- cgit v1.2.3 From bd7f86bb6adf768169fe23b63d627a252586f6b7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 14:59:00 +0200 Subject: Tried to add ctrl debug values to sdlog2 (WIP) --- .../multirotor_rate_control.c | 1 + src/modules/sdlog2/sdlog2.c | 24 +++++++++++++--------- src/modules/sdlog2/sdlog2_messages.h | 20 +++++++++--------- 3 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 01bf383e2..e8dcbacc7 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -248,4 +248,5 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, motor_skip_counter++; orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + printf("Published control debug\n"); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 844e02268..a86623304 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -91,6 +91,7 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ + printf("size: %d\n", LOG_PACKET_SIZE(_msg)); \ } else { \ log_msgs_skipped++; \ /*printf("skip\n");*/ \ @@ -1071,25 +1072,28 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + printf("copied control debug\n"); + + // log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + // log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + // log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + // log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + // log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + // log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; - log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; - log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + // log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + // log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + // log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a80af33ef..8930db33b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,25 +159,25 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; + // float roll_p; + // float roll_i; + // float roll_d; float roll_rate_p; float roll_rate_i; float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; + // float pitch_p; + // float pitch_i; + // float pitch_d; float pitch_rate_p; float pitch_rate_i; float pitch_rate_d; - float yaw_p; - float yaw_i; - float yaw_d; + // float yaw_p; + // float yaw_i; + // float yaw_d; float yaw_rate_p; float yaw_rate_i; @@ -210,7 +210,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRateP,RollRateI,RollRateD,PitchP,PitchI,PitchD,PitchRateP,PitchRateI,PitchRateD,YawP,YawI,YawD,YawRateP,YawRateI,YawRateD"), + LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; -- cgit v1.2.3 From 303694f5f7b7a03488c6075ff2bd67014badfacb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: Fixed pid bug, attitude was not controlled --- .../multirotor_att_control/multirotor_attitude_control.c | 10 ++++++---- src/modules/systemlib/pid/pid.c | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adc..4c8f72b8d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea..0188a51c4 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { ret = 1; @@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; -- cgit v1.2.3 From c189ac1c85a272142f925339879f22563c092725 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- src/modules/fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- src/modules/fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- src/modules/multirotor_att_control/multirotor_attitude_control.c | 4 ++-- src/modules/multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0..7009356b7 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e515..991d5ec5d 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603f..9c19c6786 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d..51faaa8c0 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726a..322c5485a 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4..3685b2aeb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d75..e3d9038cd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); -- cgit v1.2.3 From 2cb928d87c385335de72bb83710583a288d05cc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: Added ctrl debugging values Conflicts: src/modules/sdlog2/sdlog2.c --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 ++++++++- src/modules/sdlog2/sdlog2_messages.h | 28 ++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/vehicle_control_debug.h | 87 ++++++++++++++++++++++ 5 files changed, 165 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 322c5485a..01bf383e2 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include #include #include +#include +#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + static struct vehicle_control_debug_s control_debug; + static orb_advert_t control_debug_pub; + + + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, NULL, NULL, NULL); + rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, NULL, NULL, NULL); + rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; + + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80..347d5dd20 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -612,7 +613,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 16; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -635,6 +636,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -656,6 +658,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; } subs; @@ -675,6 +678,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; } body; @@ -762,6 +766,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -1031,6 +1041,30 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; + log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; + log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e..a3d35b596 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -156,14 +156,37 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + float pitch_p; + float pitch_i; + float pitch_d; + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; + float yaw_p; + float yaw_i; + float yaw_d; + float yaw_rate_p; + float yaw_rate_i; + float yaw_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; @@ -182,6 +205,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e22b58cad..ddf9272ec 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -116,6 +116,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 000000000..6184284a4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif -- cgit v1.2.3 From 6f108e18d21b61ea6a3bf137f01023c594085494 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 15:32:53 +0200 Subject: Just include the rate controls for now --- src/modules/sdlog2/sdlog2.c | 9 --------- src/modules/sdlog2/sdlog2_messages.h | 11 +---------- 2 files changed, 1 insertion(+), 19 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 347d5dd20..aff0fd4a7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1045,21 +1045,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; - log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; - log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a3d35b596..8dd36c74d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,21 +159,12 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; float roll_rate_p; float roll_rate_i; float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; float pitch_rate_p; float pitch_rate_i; float pitch_rate_d; - float yaw_p; - float yaw_i; - float yaw_d; float yaw_rate_p; float yaw_rate_i; float yaw_rate_d; @@ -205,7 +196,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"), + LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; -- cgit v1.2.3 From 38558f0f168ae0e486df4886533ea522c4ab18ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 16:00:44 +0200 Subject: Count and write for control debug loging was missing (still not working) --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index aff0fd4a7..fed959775 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1054,6 +1054,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); } /* --- FLOW --- */ -- cgit v1.2.3 From 216617431da72ce7b34911f63fa5958302a531e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 16:18:40 +0200 Subject: Logging of ctrl debug values working now --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fed959775..1008c149f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1044,7 +1044,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - + log_msg.msg_type = LOG_CTRL_MSG; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; -- cgit v1.2.3 From 650de90a904368eb93689bbb8a3be63888bf244e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:40:55 +0400 Subject: sdlog2: ARSP, GPOS messages added --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 27 +++++++++++++++++++++------ 2 files changed, 33 insertions(+), 12 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e93b934c8..65cfdc188 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -608,12 +608,9 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* file descriptors to wait for */ - struct pollfd fds_control[2]; - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -681,8 +678,9 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; - struct log_ARSP_s log_ARSP; struct log_AIRS_s log_AIRS; + struct log_ARSP_s log_ARSP; + struct log_GPOS_s log_GPOS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1053,7 +1051,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.hdg = buf.global_pos.hdg; + LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- VICON POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5eeebcd95..c47c388c4 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,20 +171,33 @@ struct log_OUT0_s { float output[8]; }; +/* --- AIRS - AIRSPEED --- */ +#define LOG_AIRS_MSG 13 +struct log_AIRS_s { + float indicated_airspeed; + float true_airspeed; +}; + /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 13 +#define LOG_ARSP_MSG 14 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; float yaw_rate_sp; }; -/* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 -struct log_AIRS_s { - float indicated_airspeed; - float true_airspeed; +/* --- GPOS - GLOBAL POSITION --- */ +#define LOG_GPOS_MSG 15 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; + float hdg; }; + #pragma pack(pop) /* construct list of all message formats */ @@ -203,6 +216,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3 From a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:41:35 +0400 Subject: position_estimator_inav rewrite, publishes vehicle_global_position now --- .../position_estimator_inav/inertial_filter.c | 10 +- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 305 +++++++++++---------- 3 files changed, 164 insertions(+), 153 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index b70d3504e..8cdde5e1a 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,16 +13,16 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +void inertial_filter_correct(float edt, float x[3], int i, float w) { - float e = z - x[i]; - x[i] += e * w * dt; + float ewdt = w * edt; + x[i] += ewdt; if (i == 0) { - x[1] += e * w * w * dt; + x[1] += w * ewdt; //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 } else if (i == 1) { - x[2] += e * w * w * dt; + x[2] += w * ewdt; } } diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index 18c194abf..dca6375dc 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -10,4 +10,4 @@ void inertial_filter_predict(float dt, float x[3]); -void inertial_filter_correct(float dt, float x[3], int i, float z, float w); +void inertial_filter_correct(float edt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 07ea7cd5c..ac51a7010 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -159,14 +160,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; - int baro_loop_cnt = 0; - int baro_loop_end = 70; /* measurement for 1 second */ + int baro_init_cnt = 0; + int baro_init_num = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ double lat_current = 0.0; //[°]] --> 47.0 double lon_current = 0.0; //[°]] -->8.5 double alt_current = 0.0; //[m] above MSL + uint32_t accel_counter = 0; + uint32_t baro_counter = 0; + /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); @@ -177,8 +181,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s pos; - memset(&pos, 0, sizeof(pos)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -188,79 +194,96 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); - bool local_flag_baroINITdone = false; /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - /* wait for GPS fix, only then can we initialize the projection */ - if (params.use_gps) { - struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } - }; - - while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { - if (fds_init[0].revents & POLLIN) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - } - } + struct pollfd fds_init[2] = { + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; - static int printcounter = 0; + /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ + bool wait_gps = params.use_gps; + bool wait_baro = true; - if (printcounter == 100) { - printcounter = 0; - warnx("waiting for GPS fix type 3..."); - } + while (wait_gps || wait_baro) { + if (poll(fds_init, 2, 1000)) { + if (fds_init[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (wait_baro && sensor.baro_counter > baro_counter) { + /* mean calculation over several measurements */ + if (baro_init_cnt < baro_init_num) { + baro_alt0 += sensor.baro_alt_meter; + baro_init_cnt++; - printcounter++; + } else { + wait_baro = false; + baro_alt0 /= (float) baro_init_cnt; + warnx("init baro: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); + local_pos.home_alt = baro_alt0; + local_pos.home_timestamp = hrt_absolute_time(); + } + } + } + if (fds_init[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (wait_gps && gps.fix_type >= 3) { + wait_gps = false; + /* get GPS position for first initialization */ + lat_current = gps.lat * 1e-7; + lon_current = gps.lon * 1e-7; + alt_current = gps.alt * 1e-3; + + local_pos.home_lat = lat_current * 1e7; + local_pos.home_lon = lon_current * 1e7; + local_pos.home_hdg = 0.0f; + local_pos.home_timestamp = hrt_absolute_time(); + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + } + } } - - /* get GPS position for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7; - lon_current = ((double)(gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - - pos.home_lat = lat_current * 1e7; - pos.home_lon = lon_current * 1e7; - pos.home_timestamp = hrt_absolute_time(); - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ } - warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current); hrt_abstime t_prev = 0; - thread_running = true; - uint32_t accel_counter = 0; + + uint16_t accel_updates = 0; hrt_abstime accel_t = 0; - float accel_dt = 0.0f; - uint32_t baro_counter = 0; + uint16_t baro_updates = 0; hrt_abstime baro_t = 0; hrt_abstime gps_t = 0; - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; + hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float baro_corr = 0.0f; // D + float gps_corr[2][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + }; + /* main loop */ struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, @@ -269,14 +292,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; + + thread_running = true; warnx("main loop started."); while (!thread_should_exit) { - bool accelerometer_updated = false; - bool baro_updated = false; - bool gps_updated = false; - float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; - int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); @@ -314,34 +334,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { - accelerometer_updated = true; + if (att.R_valid) { + /* transform acceleration vector from body frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + accel_NED[2] += CONSTANTS_ONE_G; + accel_corr[0] = accel_NED[0] - x_est[2]; + accel_corr[1] = accel_NED[1] - y_est[2]; + accel_corr[2] = accel_NED[2] - z_est[2]; + } else { + memset(accel_corr, 0, sizeof(accel_corr)); + } accel_counter = sensor.accelerometer_counter; accel_updates++; - accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; - accel_t = t; } if (sensor.baro_counter > baro_counter) { - baro_updated = true; + baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[2]; baro_counter = sensor.baro_counter; baro_updates++; } - - /* barometric pressure estimation at start up */ - if (!local_flag_baroINITdone && baro_updated) { - /* mean calculation over several measurements */ - if (baro_loop_cnt < baro_loop_end) { - baro_alt0 += sensor.baro_alt_meter; - baro_loop_cnt++; - - } else { - baro_alt0 /= (float)(baro_loop_cnt); - local_flag_baroINITdone = true; - warnx("baro_alt0 = %.2f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0); - pos.home_alt = baro_alt0; - } - } } if (params.use_gps) { @@ -349,80 +366,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* project GPS lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double)(gps.lat)) * 1e-7, - ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), - &(proj_pos_gps[1])); - proj_pos_gps[2] = (float)(gps.alt * 1e-3); - gps_updated = true; - pos.valid = gps.fix_type >= 3; - gps_updates++; + if (gps.fix_type >= 3) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s; + gps_corr[1][1] = gps.vel_e_m_s; + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + local_pos.valid = true; + gps_updates++; + } else { + local_pos.valid = false; + } } - } else { - pos.valid = true; + local_pos.valid = true; } } /* end of poll return value check */ - float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - if (att.R_valid) { - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; + /* inertial filter correction for altitude */ + inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; - } - } - - accel_NED[2] += CONSTANTS_ONE_G; - - /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est); - - /* inertial filter correction for altitude */ - if (local_flag_baroINITdone && baro_updated) { - if (baro_t > 0) { - inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); - } - - baro_t = t; - } - - if (accelerometer_updated) { - inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); - } - - if (params.use_gps) { - /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est); - inertial_filter_predict(dt, y_est); - - /* inertial filter correction for position */ - if (gps_updated) { - if (gps_t > 0) { - float gps_dt = (t - gps_t) / 1000000.0f; - inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); - inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); - inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); - inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); - } + if (params.use_gps) { + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); - gps_t = t; - } + /* inertial filter correction for position */ + inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); - if (accelerometer_updated) { - inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); - inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); - } - } + inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); } if (verbose_mode) { @@ -445,20 +438,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; - pos.x = x_est[0]; - pos.vx = x_est[1]; - pos.y = y_est[0]; - pos.vy = y_est[1]; - pos.z = z_est[0]; - pos.vz = z_est[1]; - pos.timestamp = hrt_absolute_time(); - - if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); + local_pos.timestamp = t; + local_pos.x = x_est[0]; + local_pos.vx = x_est[1]; + local_pos.y = y_est[0]; + local_pos.vy = y_est[1]; + local_pos.z = z_est[0]; + local_pos.vz = z_est[1]; + local_pos.absolute_alt = local_pos.home_alt - local_pos.z; + local_pos.hdg = att.yaw; + + if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) + && (isfinite(local_pos.y)) + && (isfinite(local_pos.vy)) + && (isfinite(local_pos.z)) + && (isfinite(local_pos.vz))) { + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + if (params.use_gps) { + global_pos.valid = local_pos.valid; + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t) (est_lat * 1e7); + global_pos.lon = (int32_t) (est_lon * 1e7); + global_pos.alt = -local_pos.z - local_pos.home_alt; + global_pos.relative_alt = -local_pos.z; + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.vz = local_pos.vz; + global_pos.hdg = local_pos.hdg; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } } } } -- cgit v1.2.3 From 2124c52cff152e696a73888a7b16556d3c882ec1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 22:06:45 +0400 Subject: position_estimator_inav bugfixes --- src/modules/position_estimator_inav/inertial_filter.c | 2 +- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 8cdde5e1a..acaf693f1 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -20,7 +20,7 @@ void inertial_filter_correct(float edt, float x[3], int i, float w) if (i == 0) { x[1] += w * ewdt; - //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + x[2] += w * w * ewdt / 3.0; } else if (i == 1) { x[2] += w * ewdt; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ac51a7010..a8a66e93d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -355,7 +355,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[2]; + baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } -- cgit v1.2.3 From effce6edfa3b4258a855342d8f7a265f2f18f521 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 22:07:05 +0400 Subject: sdlog2 GPOS message bug fix --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b42f11e61..55bc36717 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -218,7 +218,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD,Heading"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3 From a25d68440d99b4214ae4477d07c23e852458c4d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:01:25 +0200 Subject: Merge with att_fix --- .../attitude_estimator_ekf_params.c | 48 +++++++++++----------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 7d3812abd..52dac652b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,42 +44,42 @@ /* Extended Kalman Filter covariances */ /* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ - h->q0 = param_find("EKF_ATT_V2_Q0"); - h->q1 = param_find("EKF_ATT_V2_Q1"); - h->q2 = param_find("EKF_ATT_V2_Q2"); - h->q3 = param_find("EKF_ATT_V2_Q3"); - h->q4 = param_find("EKF_ATT_V2_Q4"); + h->q0 = param_find("EKF_ATT_V3_Q0"); + h->q1 = param_find("EKF_ATT_V3_Q1"); + h->q2 = param_find("EKF_ATT_V3_Q2"); + h->q3 = param_find("EKF_ATT_V3_Q3"); + h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V2_R0"); - h->r1 = param_find("EKF_ATT_V2_R1"); - h->r2 = param_find("EKF_ATT_V2_R2"); - h->r3 = param_find("EKF_ATT_V2_R3"); + h->r0 = param_find("EKF_ATT_V3_R0"); + h->r1 = param_find("EKF_ATT_V3_R1"); + h->r2 = param_find("EKF_ATT_V3_R2"); + h->r3 = param_find("EKF_ATT_V3_R3"); - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); + h->roll_off = param_find("ATT_ROLL_OFF3"); + h->pitch_off = param_find("ATT_PITCH_OFF3"); + h->yaw_off = param_find("ATT_YAW_OFF3"); return OK; } -- cgit v1.2.3 From 52f8565f0b5326bedb8b8a970c987e6bf10d71f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:02:52 +0200 Subject: Corrected number of ORB structs in sdlog2 --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 885155e0d..9cf6640bf 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -600,7 +600,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 17; + const ssize_t fdsc = 19; /* Sanity check variable and index */ ssize_t fdsc_count = 0; -- cgit v1.2.3 From 2daff9ebbf066c0b14b85364ee056c3d8c7a7734 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:03:55 +0200 Subject: Checkpoint: Quad is flying after PID lib changes --- .../multirotor_att_control/multirotor_attitude_control.c | 6 +++--- src/modules/multirotor_att_control/multirotor_rate_control.c | 10 ++++------ 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 6da808da4..88cc65148 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -183,12 +183,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); - + att->roll, att->rollspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 8cd97d7c1..a39d6f13d 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -186,10 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } @@ -197,8 +195,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } /* control pitch (forward) output */ -- cgit v1.2.3 From c874f681080e0e68d62a31e88c80240350f8a595 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:03:55 +0200 Subject: Checkpoint: Quad is flying after PID lib changes Conflicts: src/modules/multirotor_att_control/multirotor_attitude_control.c --- .../multirotor_att_control/multirotor_attitude_control.c | 2 +- src/modules/multirotor_att_control/multirotor_rate_control.c | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 51faaa8c0..0dad10316 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -187,7 +187,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); - + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 01bf383e2..9135a9351 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -189,10 +189,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } @@ -201,8 +199,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } /* control pitch (forward) output */ -- cgit v1.2.3 From cc452834c0dabd2689f5f102ce1cbbe714f056dd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 00:30:10 +0200 Subject: First try to prevent motors from stopping when armed --- src/drivers/px4io/px4io.cpp | 108 ++++++++++++++++++++++++++++++++++ src/modules/px4iofirmware/mixer.cpp | 20 ++++++- src/modules/px4iofirmware/protocol.h | 6 ++ src/modules/px4iofirmware/px4io.h | 2 + src/modules/px4iofirmware/registers.c | 58 ++++++++++++++++++ 5 files changed, 192 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d728b7b76..6a596a987 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -125,6 +125,16 @@ public: */ int set_failsafe_values(const uint16_t *vals, unsigned len); + /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -711,6 +721,34 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); } +int +PX4IO::set_min_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); +} + +int +PX4IO::set_max_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); +} + + + int PX4IO::io_set_arming_state() { @@ -1792,6 +1830,76 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "min")) { + + if (argc < 3) { + errx(1, "min command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 900. */ + uint16_t min[8]; + + for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + { + /* set channel to commanline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + min[i] = atoi(argv[i+2]); + if (min[i] < 900 || min[i] > 1200) { + errx(1, "value out of range of 900 < value < 1200. Aborting."); + } + } else { + /* a zero value will the default */ + min[i] = 900; + } + } + + int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + + if (ret != OK) + errx(ret, "failed setting min values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + + if (!strcmp(argv[1], "max")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 2100. */ + uint16_t max[8]; + + for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + { + /* set channel to commanline argument or to 2100 for non-provided channels */ + if (argc > i + 2) { + max[i] = atoi(argv[i+2]); + if (max[i] < 1800 || max[i] > 2100) { + errx(1, "value out of range of 1800 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + max[i] = 2100; + } + } + + int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + + if (ret != OK) + errx(ret, "failed setting max values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..e257e7cb8 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -59,6 +59,11 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 +/* + * Time that the ESCs need to initialize + */ + #define ESC_INIT_TIME_US 500000 + /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -69,6 +74,8 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static uint64_t time_armed; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -176,8 +183,16 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 600.0f) + 1500; + /* scale to control range after init time */ + if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + + /* but use init range from 900 to 2100 right after arming */ + } else { + r_page_servos[i] = (outputs[i] * 600 + 1500); + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -206,6 +221,7 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + time_armed = hrt_absolute_time(); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..1c9715451 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,12 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + + /* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..3e4027c9b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -77,6 +77,8 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..1fcfb2906 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -184,6 +184,22 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -247,6 +263,42 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_min[offset] = 1200; + else if (*values < 900) + r_page_servo_control_min[offset] = 900; + else + r_page_servo_control_min[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_max[offset] = 1200; + else if (*values < 900) + r_page_servo_control_max[offset] = 900; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -583,6 +635,12 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; default: return -1; -- cgit v1.2.3 From b5f4f1ee808c176c5dc0705b76584b438f151650 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 10:00:08 +0200 Subject: Adressed performance concern and fixed a copy paste bug --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/px4iofirmware/mixer.cpp | 11 +++++++++-- src/modules/px4iofirmware/registers.c | 18 +++++++++++++----- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6a596a987..be4dd5d19 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1851,7 +1851,7 @@ px4io_main(int argc, char *argv[]) } } else { /* a zero value will the default */ - min[i] = 900; + min[i] = 0; } } @@ -1886,7 +1886,7 @@ px4io_main(int argc, char *argv[]) } } else { /* a zero value will the default */ - max[i] = 2100; + max[i] = 0; } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e257e7cb8..62a6ebf13 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,7 +62,7 @@ extern "C" { /* * Time that the ESCs need to initialize */ - #define ESC_INIT_TIME_US 500000 + #define ESC_INIT_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -75,6 +75,7 @@ extern "C" { static bool mixer_servos_armed = false; static uint64_t time_armed; +static bool init_complete = false; /* selected control values and count for mixing */ enum mixer_source { @@ -177,6 +178,10 @@ mixer_tick(void) /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + init_complete = true; + } + /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -184,7 +189,7 @@ mixer_tick(void) r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to control range after init time */ - if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + if (init_complete) { r_page_servos[i] = (outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); @@ -222,11 +227,13 @@ mixer_tick(void) up_pwm_servo_arm(true); mixer_servos_armed = true; time_armed = hrt_absolute_time(); + init_complete = false; } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + init_complete = false; } if (mixer_servos_armed) { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1fcfb2906..bc1c83901 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -268,7 +268,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) + if (*values == 0) + /* set to default */ + r_page_servo_control_min[offset] = 900; + + else if (*values > 1200) r_page_servo_control_min[offset] = 1200; else if (*values < 900) r_page_servo_control_min[offset] = 900; @@ -286,10 +290,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) - r_page_servo_control_max[offset] = 1200; - else if (*values < 900) - r_page_servo_control_max[offset] = 900; + if (*values == 0) + /* set to default */ + r_page_servo_control_max[offset] = 2100; + + else if (*values > 2100) + r_page_servo_control_max[offset] = 2100; + else if (*values < 1800) + r_page_servo_control_max[offset] = 1800; else r_page_servo_control_max[offset] = *values; -- cgit v1.2.3 From f3ce61d7405a7edc1e5bb2aadf2ccec5bed90feb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 09:35:19 +0200 Subject: Forgot to remove some debug stuff --- src/modules/px4iofirmware/mixer.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e3ec2fa76..6752a8128 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,14 +179,12 @@ mixer_tick(void) if (mixer_servos_armed && !esc_init_done && !esc_init_active) { esc_init_time = hrt_absolute_time(); esc_init_active = true; - isr_debug(1, "start counting now"); } /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { esc_init_active = false; esc_init_done = true; - isr_debug(1, "time is up"); } /* mix */ @@ -251,9 +249,7 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> disarmed"); - esc_init_active = false; - isr_debug(1, "disarming, and init aborted"); - } + esc_init_active = false; } if (mixer_servos_armed) { /* update the servo outputs. */ -- cgit v1.2.3 From e2ff60b0a6dbcd714d57e781d9fe174b110a6237 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 09:35:59 +0200 Subject: Use rollacc and pitchacc from the estimator instead of differentiating in the controller --- .../multirotor_att_control_main.c | 17 ++++++++++++----- .../multirotor_att_control/multirotor_rate_control.c | 12 ++++++------ .../multirotor_att_control/multirotor_rate_control.h | 2 +- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c3b2f5d2a..dd38242d3 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -394,7 +394,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -405,11 +406,17 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug); + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + + + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a39d6f13d..e37ede3e0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -186,8 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); } @@ -201,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -225,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 465549540..fc741c378 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ -- cgit v1.2.3 From 8d6cc86b4f37773c9c4db77b9666fa2a075c1871 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:01:16 +0200 Subject: Cherry-picked commit e2ff60b0a6dbcd714d57e781d9fe174b110a6237: use rateacc values --- .../multirotor_att_control_main.c | 26 +++++++++++++++++----- .../multirotor_rate_control.c | 22 +++++++----------- .../multirotor_rate_control.h | 4 +++- 3 files changed, 32 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c..38775ed1f 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -83,6 +84,7 @@ static int mc_task; static bool motor_test_mode = false; static orb_advert_t actuator_pub; +static orb_advert_t control_debug_pub; static struct vehicle_status_s state; @@ -107,6 +109,9 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); + struct vehicle_control_debug_s control_debug; + memset(&control_debug, 0, sizeof(control_debug)); + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -133,6 +138,8 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -372,7 +379,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -383,13 +391,21 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + - multirotor_control_rates(&rates_sp, gyro, &actuators); + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 9135a9351..e37ede3e0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -55,7 +55,7 @@ #include #include #include -#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ @@ -151,7 +151,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -178,10 +179,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; - static struct vehicle_control_debug_s control_debug; - static orb_advert_t control_debug_pub; - - /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -189,10 +186,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -205,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -229,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ @@ -244,6 +240,4 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; - - orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 03dec317a..fc741c378 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -49,8 +49,10 @@ #include #include #include +#include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ -- cgit v1.2.3 From eb38ea17896e9970e9bdf532557ebcd87f81e34a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 19 Jun 2013 12:17:10 +0400 Subject: position_estimator_inav: better handling of lost GPS, minor fixes --- .../position_estimator_inav_main.c | 60 +++++++++++----------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a8a66e93d..278a319b5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -254,7 +254,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize coordinates */ map_projection_init(lat_current, lon_current); warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); } } } @@ -361,34 +361,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (params.use_gps) { + if (params.use_gps && fds[4].revents & POLLIN) { /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; - } - local_pos.valid = true; - gps_updates++; + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s; + gps_corr[1][1] = gps.vel_e_m_s; } else { - local_pos.valid = false; + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; } + gps_updates++; + } else { + memset(gps_corr, 0, sizeof(gps_corr)); } - } else { - local_pos.valid = true; } - } /* end of poll return value check */ @@ -403,19 +396,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); - if (params.use_gps) { + /* dont't try to estimate position when no any position source available */ + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + if (can_estimate_pos) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); /* inertial filter correction for position */ - inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); - inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); + + if (params.use_gps && gps.fix_type >= 3) { + inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid) { + inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); + } + } } if (verbose_mode) { @@ -439,6 +438,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; local_pos.timestamp = t; + local_pos.valid = can_estimate_pos; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; -- cgit v1.2.3 From 53eb76f4d558d345cc85b8f30e232b9bca2f0e51 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:34:41 +0200 Subject: Fixed numeration that was introduced through merge, I should add new log messages to the end --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a83a72514..b6537b2cd 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -190,14 +190,14 @@ struct log_OUT0_s { }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; -- cgit v1.2.3 From d6c15b16792f3f087a0d934677615949c9e12688 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:34:41 +0200 Subject: Fixed numeration that was introduced through merge, I should add new log messages to the end --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index bd69445b5..2b61378ed 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -187,14 +187,14 @@ struct log_OUT0_s { }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; -- cgit v1.2.3 From c1049483a82857bebb012607578add9492446321 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 12:01:22 +0200 Subject: Added integral reset for rate controller --- src/modules/multirotor_att_control/multirotor_rate_control.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e37ede3e0..a0266e1b3 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -199,6 +199,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); -- cgit v1.2.3 From ece93ab62834be7f46501b1d31733cf58b5b1188 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 12:01:22 +0200 Subject: Added integral reset for rate controller --- src/modules/multirotor_att_control/multirotor_rate_control.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e37ede3e0..a0266e1b3 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -199,6 +199,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); -- cgit v1.2.3 From 23858a6726f151cc6d67ecda0d42c7374839d80f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 22:59:40 +0200 Subject: Added functionality to enable PWM output for stupid ESCs even when safety is not off, arming button functionality remains as is --- src/drivers/px4io/px4io.cpp | 71 +++++++++++++++++-- src/modules/px4iofirmware/mixer.cpp | 125 ++++++++++++++++++++++++---------- src/modules/px4iofirmware/protocol.h | 6 +- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 39 ++++++++++- 5 files changed, 197 insertions(+), 45 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bce193bca..0ea90beb4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -136,6 +136,11 @@ public: */ int set_max_values(const uint16_t *vals, unsigned len); + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -567,7 +572,8 @@ PX4IO::init() io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); /* publish RC config to IO */ ret = io_set_rc_config(); @@ -793,6 +799,20 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); } +int +PX4IO::set_idle_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + printf("Sending IDLE values\n"); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); +} int @@ -1441,12 +1461,14 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), @@ -1473,8 +1495,10 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\n"); + printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("idle values"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); } int @@ -1973,6 +1997,41 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "idle")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 0. */ + uint16_t idle[8]; + + for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + { + /* set channel to commanline argument or to 0 for non-provided channels */ + if (argc > i + 2) { + idle[i] = atoi(argv[i+2]); + if (idle[i] < 900 || idle[i] > 2100) { + errx(1, "value out of range of 900 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + idle[i] = 0; + } + } + + int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + + if (ret != OK) + errx(ret, "failed setting idle values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -2100,5 +2159,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6752a8128..1f118ea2f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -63,6 +63,7 @@ extern "C" { * Time that the ESCs need to initialize */ #define ESC_INIT_TIME_US 1000000 + #define ESC_RAMP_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -73,10 +74,17 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; - +static bool should_arm = false; +static bool should_always_enable_pwm = false; static uint64_t esc_init_time; -static bool esc_init_active = false; -static bool esc_init_done = false; + +enum esc_state_e { + ESC_OFF, + ESC_INIT, + ESC_RAMP, + ESC_ON +}; +static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -175,18 +183,48 @@ mixer_tick(void) float outputs[IO_SERVO_COUNT]; unsigned mixed; - /* after arming, some ESCs need an initalization period, count the time from here */ - if (mixer_servos_armed && !esc_init_done && !esc_init_active) { - esc_init_time = hrt_absolute_time(); - esc_init_active = true; + uint16_t ramp_promille; + + /* update esc init state, but only if we are truely armed and not just PWM enabled */ + if (mixer_servos_armed && should_arm) { + + switch (esc_state) { + + /* after arming, some ESCs need an initalization period, count the time from here */ + case ESC_OFF: + esc_init_time = hrt_absolute_time(); + esc_state = ESC_INIT; + break; + + /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ + case ESC_INIT: + if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { + esc_state = ESC_RAMP; + } + break; + + /* then ramp until the min speed is reached */ + case ESC_RAMP: + if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { + esc_state = ESC_ON; + } + break; + + case ESC_ON: + default: + + break; + } + } else { + esc_state = ESC_OFF; } - /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ - if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { - esc_init_active = false; - esc_init_done = true; + /* do the calculations during the ramp for all at once */ + if(esc_state == ESC_RAMP) { + ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; } + /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); @@ -196,21 +234,27 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* XXX maybe this check for an armed FMU could be achieved a little less messy */ - if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) { - r_page_servos[i] = r_page_servo_failsafe[i]; - } - /* during ESC initialization, use low PWM */ - else if (esc_init_active) { - r_page_servos[i] = (outputs[i] * 600 + 1500); - - /* afterwards use min and max values */ - } else { - r_page_servos[i] = (outputs[i] - * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 - + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + switch (esc_state) { + case ESC_INIT: + r_page_servos[i] = (outputs[i] * 600 + 1500); + break; + + case ESC_RAMP: + r_page_servos[i] = (outputs[i] + * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 + + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); + break; + + case ESC_ON: + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + break; + + case ESC_OFF: + default: + break; } - } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) r_page_servos[i] = 0; @@ -225,36 +269,43 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = ( + should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) || - - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) + /* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + /* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) ); + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - if (should_arm && !mixer_servos_armed) { + if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; - isr_debug(5, "> armed"); + isr_debug(5, "> PWM enabled"); - } else if (!should_arm && mixer_servos_armed) { + } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); - isr_debug(5, "> disarmed"); + isr_debug(5, "> PWM disabled"); - esc_init_active = false; } + } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + + } else if (mixer_servos_armed && should_always_enable_pwm) { + /* set the idle servo outputs. */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_idle[i]); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b43054197..0e477a200 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -155,6 +155,7 @@ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ @@ -190,9 +191,12 @@ /* PWM minimum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - /* PWM maximum values for certain ESCs */ +/* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM idle values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 3e4027c9b..042e7fe66 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -79,6 +79,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 379cf09e3..bd13f3b7d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -147,7 +147,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -201,6 +202,14 @@ uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, */ uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +/** + * PAGE 108 + * + * idle PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_idle[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -308,6 +317,31 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_IDLE_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_idle[offset] = 0; + + else if (*values < 900) + r_page_servo_idle[offset] = 900; + else if (*values > 2100) + r_page_servo_idle[offset] = 2100; + else + r_page_servo_idle[offset] = *values; + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -651,6 +685,9 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_IDLE_PWM: + SELECT_PAGE(r_page_servo_idle); + break; default: return -1; -- cgit v1.2.3 From 9b6c9358ed072459ac61feed271a209c8c5dea23 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Jun 2013 01:13:49 +0200 Subject: First try for an ESC calibration tool --- makefiles/config_px4fmu_default.mk | 1 + src/drivers/px4io/px4io.cpp | 5 +- src/modules/px4iofirmware/mixer.cpp | 13 +- src/systemcmds/esc_calib/esc_calib.c | 250 +++++++++++++++++++++++++++++++++++ src/systemcmds/esc_calib/module.mk | 41 ++++++ 5 files changed, 303 insertions(+), 7 deletions(-) create mode 100644 src/systemcmds/esc_calib/esc_calib.c create mode 100644 src/systemcmds/esc_calib/module.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 43288537c..e8104b351 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -46,6 +46,7 @@ MODULES += systemcmds/param MODULES += systemcmds/perf MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ea90beb4..6d3c32ed9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1496,9 +1496,10 @@ PX4IO::print_status() printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("idle values"); + printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf("\n"); } int diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1f118ea2f..fe9166779 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -270,11 +270,14 @@ mixer_tick(void) * here. */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c new file mode 100644 index 000000000..188fa4e37 --- /dev/null +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * 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 esc_calib.c + * + * Tool for ESC calibration + */ + +#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 esc_calib_main(int argc, char *argv[]); + +#define MAX_CHANNELS 8 + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "esc_calib [-d ] \n" + " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " Provide channels (e.g.: 1 2 3 4)\n" + ); + +} + +int +esc_calib_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + char *ep; + bool channels_selected[MAX_CHANNELS] = {false}; + int ch; + int ret; + char c; + + if (argc < 2) + usage(NULL); + + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + + case 'd': + dev = optarg; + argc--; + break; + + default: + usage(NULL); + } + } + + if(argc < 1) { + usage("no channels provided"); + } + + while (--argc) { + const char *arg = argv[argc]; + unsigned channel_number = strtol(arg, &ep, 0); + + if (*ep == '\0') { + if (channel_number > MAX_CHANNELS || channel_number <= 0) { + err(1, "invalid channel number: %d", channel_number); + } else { + channels_selected[channel_number-1] = true; + + } + } + } + + /* Wait for confirmation */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("ATTENTION, please remove or fix props before starting calibration!\n" + "\n" + "Also press the arming switch first for safety off\n" + "\n" + "Do you really want to start calibration: y or n?\n"); + + /* wait for user input */ + while (1) { + + if (read(console, &c, 1) == 1) { + if (c == 'y' || c == 'Y') { + + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("ESC calibration exited"); + close(console); + exit(0); + } else if (c == 'n' || c == 'N') { + warnx("ESC calibration aborted"); + close(console); + exit(0); + } else { + warnx("Unknown input, ESC calibration aborted"); + close(console); + exit(0); + } + } + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + // XXX arming not necessaire at the moment + // /* Then arm */ + // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + // if (ret != OK) + // err(1, "PWM_SERVO_SET_ARM_OK"); + + // ret = ioctl(fd, PWM_SERVO_ARM, 0); + // if (ret != OK) + // err(1, "PWM_SERVO_ARM"); + + + + + /* Wait for user confirmation */ + warnx("Set high PWM\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step"); + + while (1) { + + /* First set high PWM */ + for (unsigned i = 0; i Date: Thu, 20 Jun 2013 11:30:10 +0400 Subject: Att rate PID fix --- src/modules/multirotor_att_control/multirotor_rate_control.c | 9 +++++---- src/modules/systemlib/pid/pid.c | 12 ++++++++---- src/modules/systemlib/pid/pid.h | 5 ++++- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a0266e1b3..8f26a2014 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -179,6 +179,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + float diff_filter_factor = 1.0f; /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -186,8 +187,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); } @@ -195,8 +196,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); } /* reset integral if on ground */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 3685b2aeb..ada364e37 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -144,15 +144,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - - float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; + float error_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { -// d = (error_filtered - pid->error_previous_filtered) / dt; - d = error_filtered - pid->error_previous_filtered ; + error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt + pid->error_previous_filtered = error_filtered; + } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + + error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index e3d9038cd..eb9df96cc 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -47,8 +47,11 @@ /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error * val_dot in pid_calculate() will be ignored */ #define PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC_NO_SP 1 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 1 +#define PID_MODE_DERIVATIV_SET 2 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) #define PID_MODE_DERIVATIV_NONE 9 -- cgit v1.2.3 From 462a9c84540e9258a5ab4270b258e5809cb5f88b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 11:32:25 +0400 Subject: sdlog2: add angular accelerations to ATT message --- src/modules/sdlog2/sdlog2.c | 3 +++ src/modules/sdlog2/sdlog2_messages.h | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f89b49acf..3c3f1a074 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -986,6 +986,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.roll_acc = buf.att.rollacc; + log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc; + log_msg.body.log_ATT.yaw_acc = buf.att.yawacc; LOGBUFFER_WRITE_AND_COUNT(ATT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b61378ed..43645c302 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -63,6 +63,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float roll_acc; + float pitch_acc; + float yaw_acc; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -206,7 +209,7 @@ struct log_ARSP_s { static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), -- cgit v1.2.3 From dec1fdbde0c7bb6f3eacae97ab9656f77294cbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 11:52:05 +0400 Subject: Cleanup: remove useless angular rates from attitude rate controller --- src/modules/multirotor_att_control/multirotor_att_control_main.c | 8 +------- src/modules/multirotor_att_control/multirotor_rate_control.c | 6 +++--- src/modules/multirotor_att_control/multirotor_rate_control.h | 2 +- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 38775ed1f..8ba3241ad 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -395,13 +395,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - rates_acc[0] = att.rollacc; - rates_acc[1] = att.pitchacc; - rates_acc[2] = att.yawacc; - - - - multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); + multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 8f26a2014..641d833f0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -208,11 +208,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index fc741c378..465549540 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ -- cgit v1.2.3 From bf0de775329acfc8c450b2958222a83f2a32f977 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 12:33:35 +0400 Subject: Critical bugfix in PID --- src/modules/systemlib/pid/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index ada364e37..2aafe0636 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -155,7 +155,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { - error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor; + error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { -- cgit v1.2.3 From 7d37c2a8b36309f27d7001ba035d30c72620ba05 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 17:32:29 +0400 Subject: multirotor_pos_control: bugs fixed --- .../multirotor_pos_control/multirotor_pos_control.c | 14 +++++++------- .../multirotor_pos_control/multirotor_pos_control_params.c | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c94972e7d..508879ae2 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -302,14 +302,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { + float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; - pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; + pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; local_pos_sp.x += pos_sp_speed_x * dt; local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7c3296cae..d284de433 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -68,7 +68,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); - h->slope_max = param_find("MPC_HARD"); + h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); -- cgit v1.2.3 From 53dec130c49f69f10527ab55d69de46ee26c58f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:49:46 +0200 Subject: Adapted flow estimator, position and velocity control to new state machine --- .../flow_position_control_main.c | 21 +++++++------ .../flow_position_estimator_main.c | 36 +++++++++++++--------- .../flow_speed_control/flow_speed_control_main.c | 22 +++++++------ .../multirotor_att_control_main.c | 2 +- .../multirotor_attitude_control.c | 8 ++--- 5 files changed, 50 insertions(+), 39 deletions(-) diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 5246ea62b..ecc133f63 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; struct filtered_bottom_flow_s filtered_flow; @@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -268,9 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); -// XXX fix this -#if 0 - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 @@ -492,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* store actual height for speed estimation */ last_local_pos_z = local_pos.z; - speed_sp.thrust_sp = thrust_control; + speed_sp.thrust_sp = thrust_control; //manual.throttle; speed_sp.timestamp = hrt_absolute_time(); /* publish new speed setpoint */ @@ -529,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } -#endif /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); @@ -578,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 2389c693d..8f84307a7 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) printf("[flow position estimator] starting\n"); /* rotation matrix for transformation of optical flow speed vectors */ - static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 }, - { -1, 0, 0 }, + static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, + { 1, 0, 0 }, { 0, 0, 1 }}; // 90deg rotated const float time_scale = powf(10.0f,-6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; @@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to vehicle status */ - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + + /* subscribe to safety topic */ + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to attitude */ int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -218,8 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -// XXX fix this -#if 0 + if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -265,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ float sonar_new = flow.ground_distance_m; @@ -278,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -350,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !vstatus.flag_system_armed) + if (!vehicle_liftoff || !safety.armed) { /* not possible to fly */ sonar_valid = false; @@ -440,7 +446,6 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } counter++; - #endif } printf("[flow position estimator] exiting.\n"); @@ -448,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 0be4b3d5a..4f60e34f2 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -186,7 +189,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -227,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of the safety topic */ + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of bodyframe speed setpoint */ orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { /* calc new roll/pitch */ float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; @@ -341,7 +345,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } -#endif } printf("[flow speed control] ending now...\n"); @@ -352,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(att_sp_pub); perf_print_counter(mc_loop_perf); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 41a9f1df5..4467d2d82 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -331,7 +331,7 @@ mc_thread_main(int argc, char *argv[]) // */ // /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { rates_sp.yaw = manual.yaw; control_yaw_position = false; first_time_after_yaw_speed_control = true; diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 752f292e3..4a1129e1f 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -156,8 +156,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -168,8 +168,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); } /* reset integral if on ground */ -- cgit v1.2.3 From 60e9ea977d90a05826c86eea14f4a2807851d49c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:50:55 +0200 Subject: Conditions where set wrongly in the first 2s after boot --- src/modules/commander/commander.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index edcdf7e54..9d3adaa1d 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1658,7 +1658,7 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well @@ -1666,7 +1666,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well @@ -1675,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000) { + if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_airspeed_valid = true; } else { -- cgit v1.2.3 From d563960267ab1145d5100f9dcfad6035205e4021 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:04 +0200 Subject: Added Flow messages to sdlog2 --- src/modules/sdlog2/sdlog2.c | 10 ++++++++-- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index edcba4e7d..33b3c657b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -616,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 19; + const ssize_t fdsc = 20; /* Sanity check variable and index */ ssize_t fdsc_count = 0; @@ -686,6 +686,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1129,7 +1130,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - // TODO not implemented yet + log_msg.msg_type = LOG_FLOW_MSG; + log_msg.body.log_FLOW.flow_comp_x_m = buf.flow.flow_comp_x_m; + log_msg.body.log_FLOW.flow_comp_y_m = buf.flow.flow_comp_y_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.quality = buf.flow.quality; + LOGBUFFER_WRITE_AND_COUNT(FLOW); } /* --- RC CHANNELS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 04eae40b0..755d21d55 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -206,6 +206,15 @@ struct log_ARSP_s { float pitch_rate_sp; float yaw_rate_sp; }; + +/* --- FLOW - FLOW DATA --- */ +#define LOG_FLOW_MSG 16 +struct log_FLOW_s { + float flow_comp_x_m; + float flow_comp_y_m; + float ground_distance_m; + uint8_t quality; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -226,6 +235,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(FLOW, "fffB", "FlowX,FlowY,SonAlt,FQual") }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3 From 794a2248f02e014bc81e977da5a4eee7d71902b5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:49 +0200 Subject: Only some small cleanup to att control --- .../multirotor_att_control_main.c | 100 +++++---------------- 1 file changed, 23 insertions(+), 77 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 4467d2d82..25447aaba 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -256,7 +256,7 @@ mc_thread_main(int argc, char *argv[]) } - } else if (control_mode.flag_control_manual_enabled) { + } else if (control_mode.flag_control_manual_enabled) { if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || @@ -268,90 +268,34 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ -/* XXX fix this */ -#if 0 - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; + rc_loss_first_time = true; - rc_loss_first_time = false; + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - } else { -#endif - rc_loss_first_time = true; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && safety.armed) { + att_sp.yaw_body = att.yaw; + } - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; - /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + } else { + if (first_time_after_yaw_speed_control) { att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ -#warning fix this -// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || -// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { -// -// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// -// } else { -// /* -// * This mode SHOULD be the default mode, which is: -// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS -// * -// * However, we fall back to this setting for all other (nonsense) -// * settings as well. -// */ -// - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } -// } -// } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); -#if 0 + control_yaw_position = true; } -#endif + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -369,7 +313,9 @@ mc_thread_main(int argc, char *argv[]) } } else { -#warning fix this + + //XXX TODO add acro mode here + /* manual rate inputs, from RC control or joystick */ // if (state.flag_control_rates_enabled && // state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { -- cgit v1.2.3 From 2096da2d4da2d4482dcdb328d35255101fc722bd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 13:11:15 +0200 Subject: Don't interrupt a playing tune unless its a repeated one --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ac5511e60..e8c6fd542 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -241,6 +241,8 @@ private: static const unsigned _default_ntunes; static const uint8_t _note_tab[]; + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + const char *_user_tune; const char *_tune; // current tune string @@ -456,6 +458,11 @@ const char * const ToneAlarm::_default_tunes[] = { "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64", + "MNT75L1O2G", //arming warning + "MBNT100a8", //battery warning slow + "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition }; const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); @@ -471,6 +478,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : CDev("tone_alarm", "/dev/tone_alarm"), + _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), _next(nullptr) @@ -803,8 +811,12 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); - if (_repeat) + if (_repeat) { start_tune(_tune); + } else { + _tune = nullptr; + _default_tune_number = 0; + } return; } @@ -873,8 +885,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - // play the selected tune - start_tune(_default_tunes[arg - 1]); + /* don't interrupt alarms unless they are repeated */ + if (_tune != nullptr && !_repeat) { + /* abort and let the current tune finish */ + result = -EBUSY; + } else if (_repeat && _default_tune_number == arg) { + /* requested repeating tune already playing */ + } else { + // play the selected tune + _default_tune_number = arg; + start_tune(_default_tunes[arg - 1]); + } } } else { result = -EINVAL; -- cgit v1.2.3 From a6ba7e448586c556009132887503e6830c20029e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 13:15:38 +0200 Subject: Dropped superseded safety topic, added warning tones before arming --- src/drivers/px4io/px4io.cpp | 15 +++++--- src/modules/commander/commander.c | 64 +++++++++++++++++++++---------- src/modules/uORB/objects_common.cpp | 3 -- src/modules/uORB/topics/actuator_safety.h | 2 +- src/modules/uORB/topics/safety.h | 60 ----------------------------- 5 files changed, 53 insertions(+), 91 deletions(-) delete mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d3c32ed9..df2f18270 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -81,7 +81,6 @@ #include #include #include -#include #include #include @@ -990,20 +989,24 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct safety_s safety; + struct actuator_safety_s safety; safety.timestamp = hrt_absolute_time(); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - safety.status = SAFETY_STATUS_UNLOCKED; + safety.safety_off = true; + safety.safety_switch_available = true; } else { - safety.status = SAFETY_STATUS_SAFE; + safety.safety_off = false; + safety.safety_switch_available = true; } /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(safety), _to_safety, &safety); + orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(safety), &safety); + _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); } return ret; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 9d3adaa1d..4c884aed3 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Petri Tanskanen * Lorenz Meier * Thomas Gubler @@ -79,7 +79,6 @@ #include #include #include -#include #include #include @@ -1159,6 +1158,9 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool home_position_set = false; + bool battery_tune_played = false; + bool arm_tune_played = false; + /* set parameters */ failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); @@ -1248,6 +1250,9 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + /* but also subscribe to it */ + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ @@ -1477,6 +1482,13 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update safety topic */ + orb_check(safety_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* update global position estimate */ orb_check(global_position_sub, &new_data); @@ -1573,25 +1585,6 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { - /* For less than 20%, start be slightly annoying at 1 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - tune_positive(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - /* Check battery voltage */ /* write to sys_status */ if (battery_voltage_valid) { @@ -2194,6 +2187,8 @@ int commander_thread_main(int argc, char *argv[]) } + + current_status.counter++; current_status.timestamp = hrt_absolute_time(); @@ -2219,6 +2214,33 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; + + + /* play tone according to evaluation result */ + /* check if we recently armed */ + if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + ioctl(buzzer, TONE_SET_ALARM, 12); + arm_tune_played = true; + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + } else if (bat_remain < 0.1f && battery_voltage_valid) { + ioctl(buzzer, TONE_SET_ALARM, 14); + battery_tune_played = true; + } else if (bat_remain < 0.2f && battery_voltage_valid) { + ioctl(buzzer, TONE_SET_ALARM, 13); + battery_tune_played = true; + } else if(battery_tune_played) { + ioctl(buzzer, TONE_SET_ALARM, 0); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + /* XXX use this voltage_previous */ fflush(stdout); counter++; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b5dafd0ca..b602b1714 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -169,6 +169,3 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); -/* status of the system safety device */ -#include "topics/safety.h" -ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index 3a107d41a..c431217ab 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -53,7 +53,7 @@ struct actuator_safety_s { uint64_t timestamp; - + bool safety_switch_available; /**< Set to true if a safety switch is connected */ bool safety_off; /**< Set to true if safety is off */ bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h deleted file mode 100644 index 19e8e8d45..000000000 --- a/src/modules/uORB/topics/safety.h +++ /dev/null @@ -1,60 +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 safety.h - * - * Status of an attached safety device - */ - -#ifndef TOPIC_SAFETY_H -#define TOPIC_SAFETY_H - -#include -#include "../uORB.h" - -enum SAFETY_STATUS { - SAFETY_STATUS_NOT_PRESENT, - SAFETY_STATUS_SAFE, - SAFETY_STATUS_UNLOCKED -}; - -struct safety_s { - uint64_t timestamp; /**< output timestamp in us since system boot */ - enum SAFETY_STATUS status; -}; - -/* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(safety); - -#endif /* TOPIC_SAFETY_H */ -- cgit v1.2.3 From 9ce2b62eb57b519348c4b2fcd58af09999e504e7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 14:45:27 +0200 Subject: Beep when arming or disarming with RC --- src/modules/commander/commander.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4c884aed3..4a5eb23ad 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -2033,9 +2033,11 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + tune_positive(); } else { mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + tune_negative(); } stick_off_counter = 0; @@ -2050,6 +2052,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; + tune_positive(); } else { stick_on_counter++; @@ -2219,17 +2222,16 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - ioctl(buzzer, TONE_SET_ALARM, 12); - arm_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + arm_tune_played = true; - // // XXX Export patterns and threshold to parameters /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - ioctl(buzzer, TONE_SET_ALARM, 14); - battery_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - ioctl(buzzer, TONE_SET_ALARM, 13); - battery_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + battery_tune_played = true; } else if(battery_tune_played) { ioctl(buzzer, TONE_SET_ALARM, 0); battery_tune_played = false; -- cgit v1.2.3 From 0ecc9c4bf4f2bf9fe1d99b5cbdf398718d2dccdd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 16:30:35 +0200 Subject: Shrinking the main commander file a bit --- src/modules/commander/accelerometer_calibration.c | 14 +- src/modules/commander/accelerometer_calibration.h | 1 - src/modules/commander/airspeed_calibration.c | 111 +++++ src/modules/commander/airspeed_calibration.h | 46 ++ src/modules/commander/baro_calibration.c | 54 +++ src/modules/commander/baro_calibration.h | 46 ++ src/modules/commander/calibration_routines.c | 1 + src/modules/commander/commander.c | 513 ++-------------------- src/modules/commander/commander.h | 4 - src/modules/commander/commander_helper.c | 129 ++++++ src/modules/commander/commander_helper.h | 66 +++ src/modules/commander/gyro_calibration.c | 151 +++++++ src/modules/commander/gyro_calibration.h | 46 ++ src/modules/commander/mag_calibration.c | 278 ++++++++++++ src/modules/commander/mag_calibration.h | 46 ++ src/modules/commander/module.mk | 8 +- src/modules/commander/rc_calibration.c | 83 ++++ src/modules/commander/rc_calibration.h | 46 ++ src/modules/commander/state_machine_helper.c | 68 +-- src/modules/commander/state_machine_helper.h | 12 +- 20 files changed, 1159 insertions(+), 564 deletions(-) create mode 100644 src/modules/commander/airspeed_calibration.c create mode 100644 src/modules/commander/airspeed_calibration.h create mode 100644 src/modules/commander/baro_calibration.c create mode 100644 src/modules/commander/baro_calibration.h create mode 100644 src/modules/commander/commander_helper.c create mode 100644 src/modules/commander/commander_helper.h create mode 100644 src/modules/commander/gyro_calibration.c create mode 100644 src/modules/commander/gyro_calibration.h create mode 100644 src/modules/commander/mag_calibration.c create mode 100644 src/modules/commander/mag_calibration.h create mode 100644 src/modules/commander/rc_calibration.c create mode 100644 src/modules/commander/rc_calibration.h diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 231739962..7bae8ad40 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -70,15 +70,25 @@ */ #include "accelerometer_calibration.h" +#include "commander_helper.h" +#include +#include #include +#include +#include +#include +#include + + #include #include #include #include +#include +#include #include -void do_accel_calibration(int mavlink_fd); int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -355,7 +365,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) + if (det == 0.0f) return ERROR; // Singular matrix dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 6a920c4ef..4175a25f4 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -9,7 +9,6 @@ #define ACCELEROMETER_CALIBRATION_H_ #include -#include void do_accel_calibration(int mavlink_fd); diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c new file mode 100644 index 000000000..feaf11aee --- /dev/null +++ b/src/modules/commander/airspeed_calibration.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 airspeed_calibration.c + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h new file mode 100644 index 000000000..92f5651ec --- /dev/null +++ b/src/modules/commander/airspeed_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 gyro_calibration.h + * Airspeed sensor calibration routine + */ + +#ifndef AIRSPEED_CALIBRATION_H_ +#define AIRSPEED_CALIBRATION_H_ + +#include + +void do_airspeed_calibration(int mavlink_fd); + +#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c new file mode 100644 index 000000000..a70594794 --- /dev/null +++ b/src/modules/commander/baro_calibration.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 baro_calibration.c + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +void do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return; +} diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h new file mode 100644 index 000000000..ac0f4be36 --- /dev/null +++ b/src/modules/commander/baro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 mag_calibration.h + * Barometer calibration routine + */ + +#ifndef BARO_CALIBRATION_H_ +#define BARO_CALIBRATION_H_ + +#include + +void do_baro_calibration(int mavlink_fd); + +#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c index a26938637..799cd4269 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.c @@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4a5eb23ad..41baa34d5 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -91,15 +91,15 @@ #include #include +#include "commander_helper.h" #include "state_machine_helper.h" - -#include -#include -#include -#include - #include "calibration_routines.h" #include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ @@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); extern struct system_load_s system_load; +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -130,7 +133,6 @@ extern struct system_load_s system_load; /* File descriptors */ static int leds; -static int buzzer; static int mavlink_fd; /* flags */ @@ -167,27 +169,17 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int buzzer_init(void); -void buzzer_deinit(void); int led_init(void); void led_deinit(void); int led_toggle(int led); int led_on(int led); int led_off(int led); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); void do_reboot(void); -void do_gyro_calibration(void); -void do_mag_calibration(void); -void do_rc_calibration(void); -void do_airspeed_calibration(void); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); +// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -206,23 +198,6 @@ void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void buzzer_deinit() -{ - close(buzzer); -} - int led_init() { @@ -268,27 +243,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, 2); -} - -void tune_positive() -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_neutral() -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void tune_negative() -{ - ioctl(buzzer, TONE_SET_ALARM, 5); -} - void do_reboot() { mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); @@ -298,424 +252,6 @@ void do_reboot() } -void do_rc_calibration() -{ - mavlink_log_info(mavlink_fd, "trim calibration starting"); - - /* XXX fix this */ - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - tune_positive(); - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration() -{ - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_neutral(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - close(sub_mag); -} - -void do_gyro_calibration() -{ - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - - -void do_airspeed_calibration() -{ - /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_positive(); - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - close(diff_pres_sub); -} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { @@ -1182,7 +718,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Failed to initialize leds"); } - if (buzzer_init() != 0) { + if (buzzer_init() != OK) { warnx("ERROR: Failed to initialize buzzer"); } @@ -1246,7 +782,12 @@ int commander_thread_main(int argc, char *argv[]) /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); @@ -1715,7 +1256,7 @@ int commander_thread_main(int argc, char *argv[]) // state_changed = true; // } - orb_check(ORB_ID(vehicle_gps_position), &new_data); + orb_check(gps_sub, &new_data); if (new_data) { @@ -2222,18 +1763,18 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + if (tune_arm() == OK) arm_tune_played = true; /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { - ioctl(buzzer, TONE_SET_ALARM, 0); + tune_stop(); battery_tune_played = false; } @@ -2305,25 +1846,25 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_GYRO_CALIBRATION: - do_gyro_calibration(); + do_gyro_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: - do_mag_calibration(); + do_mag_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - // do_baro_calibration(); + // do_baro_calibration(mavlink_fd); case LOW_PRIO_TASK_RC_CALIBRATION: - // do_rc_calibration(); + // do_rc_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; @@ -2337,7 +1878,7 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - do_airspeed_calibration(); + do_airspeed_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h index 04b4e72ab..6e57c0ba5 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander.h @@ -49,10 +49,6 @@ #ifndef COMMANDER_H_ #define COMMANDER_H_ -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f -void tune_confirm(void); -void tune_error(void); #endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c new file mode 100644 index 000000000..a75b5dec3 --- /dev/null +++ b/src/modules/commander/commander_helper.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 commander_helper.c + * Commander helper functions implementations + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "commander_helper.h" + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, 12); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 14); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, 0); +} + diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h new file mode 100644 index 000000000..ea96d72a6 --- /dev/null +++ b/src/modules/commander/commander_helper.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 commander_helper.h + * Commander helper functions definitions + */ + +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ + +#include +#include +#include +#include + + +bool is_multirotor(const struct vehicle_status_s *current_status); +bool is_rotary_wing(const struct vehicle_status_s *current_status); + +int buzzer_init(void); +void buzzer_deinit(void); + +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +int tune_arm(void); +int tune_critical_bat(void); +int tune_low_bat(void); + +void tune_stop(void); + +#endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c new file mode 100644 index 000000000..f452910c9 --- /dev/null +++ b/src/modules/commander/gyro_calibration.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 gyro_calibration.c + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h new file mode 100644 index 000000000..cd262507d --- /dev/null +++ b/src/modules/commander/gyro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 gyro_calibration.h + * Gyroscope calibration routine + */ + +#ifndef GYRO_CALIBRATION_H_ +#define GYRO_CALIBRATION_H_ + +#include + +void do_gyro_calibration(int mavlink_fd); + +#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c new file mode 100644 index 000000000..dbd31a7e7 --- /dev/null +++ b/src/modules/commander/mag_calibration.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * 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 mag_calibration.c + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h new file mode 100644 index 000000000..fd2085f14 --- /dev/null +++ b/src/modules/commander/mag_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 mag_calibration.h + * Magnetometer calibration routine + */ + +#ifndef MAG_CALIBRATION_H_ +#define MAG_CALIBRATION_H_ + +#include + +void do_mag_calibration(int mavlink_fd); + +#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fe44e955a..fef8e366b 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -38,6 +38,12 @@ MODULE_COMMAND = commander SRCS = commander.c \ state_machine_helper.c \ + commander_helper.c \ calibration_routines.c \ - accelerometer_calibration.c + accelerometer_calibration.c \ + gyro_calibration.c \ + mag_calibration.c \ + baro_calibration.c \ + rc_calibration.c \ + airspeed_calibration.c diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c new file mode 100644 index 000000000..9aa6b86fe --- /dev/null +++ b/src/modules/commander/rc_calibration.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 rc_calibration.c + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include + + +void do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} \ No newline at end of file diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h new file mode 100644 index 000000000..6505c7364 --- /dev/null +++ b/src/modules/commander/rc_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 rc_calibration.h + * Remote Control calibration routine + */ + +#ifndef RC_CALIBRATION_H_ +#define RC_CALIBRATION_H_ + +#include + +void do_rc_calibration(int mavlink_fd); + +#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 87aad6270..c15fc91a0 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -54,21 +54,8 @@ #include #include "state_machine_helper.h" -#include "commander.h" +#include "commander_helper.h" -bool is_multirotor(const struct vehicle_status_s *current_status) -{ - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); -} - -bool is_rotary_wing(const struct vehicle_status_s *current_status) -{ - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); -} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { @@ -568,7 +555,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (valid_transition) { current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); @@ -579,50 +570,6 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ -// if (current_status->flag_control_rates_enabled) { -// current_status->onboard_control_sensors_present |= 0x400; -// -// } else { -// current_status->onboard_control_sensors_present &= ~0x400; -// } - -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; -// -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -} - -// void publish_armed_status(const struct vehicle_status_s *current_status) -// { -// struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; -// -// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ -// /* XXX allow arming only if core sensors are ok */ -// armed.ready_to_arm = true; -// -// /* lock down actuators if required, only in HIL */ -// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; -// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); -// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -// } - - // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status // */ @@ -805,3 +752,4 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b015c4efe..b553a4b56 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -50,15 +50,7 @@ #include -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -bool is_multirotor(const struct vehicle_status_s *current_status); - -bool is_rotary_wing(const struct vehicle_status_s *current_status); - -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -- cgit v1.2.3 From 7958e38c427309941e43a17e987bb04504aa57df Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Tue, 25 Jun 2013 13:44:42 -0700 Subject: Enabled NSH on USB by default. --- nuttx/configs/px4fmuv2/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 307c5079c..8e69f321a 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -237,7 +237,7 @@ CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y CONFIG_STANDARD_SERIAL=y -CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 CONFIG_USART1_TXBUFSIZE=512 @@ -555,7 +555,7 @@ CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y +CONFIG_DEV_CONSOLE=n CONFIG_DEV_LOWCONSOLE=n CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y @@ -943,7 +943,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512 # Size of the serial receive/transmit buffers. Default 256. # CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +CONFIG_CDCACM_CONSOLE=y #CONFIG_CDCACM_EP0MAXPACKET CONFIG_CDCACM_EPINTIN=1 #CONFIG_CDCACM_EPINTIN_FSSIZE -- cgit v1.2.3 From b1f3a5c92bd900ad15d4f13f43658563be5ed8de Mon Sep 17 00:00:00 2001 From: Sam Kelly Date: Tue, 25 Jun 2013 14:01:27 -0700 Subject: Enabled MS5611 by default on FMUv2. --- makefiles/config_px4fmuv2_default.mk | 2 +- src/drivers/ms5611/ms5611.cpp | 1281 ++++++++++++++++++++++++++++------ 2 files changed, 1070 insertions(+), 213 deletions(-) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 26c249901..bad53aa42 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -21,7 +21,7 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 #MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 -#MODULES += drivers/ms5611 +MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936e..76acf7ccd 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,12 +33,13 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. */ #include #include +#include #include #include @@ -76,6 +77,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + /** * Calibration PROM as reported by the device. */ @@ -100,11 +106,11 @@ union ms5611_prom_u { }; #pragma pack(pop) -class MS5611 : public device::I2C +class MS5611_I2C : public device::I2C { public: - MS5611(int bus); - virtual ~MS5611(); + MS5611_I2C(int bus); + virtual ~MS5611_I2C(); virtual int init(); @@ -118,8 +124,6 @@ public: protected: virtual int probe(); - -private: union ms5611_prom_u _prom; struct work_s _work; @@ -148,6 +152,85 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + /** + * Initialize the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + virtual int measure(); + + /** + * Collect the result of the most recent measurement. + */ + virtual int collect(); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + virtual int cmd_reset(); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + virtual int read_prom(); + + /** + * Execute the bus-specific ioctl (I2C or SPI) + * + * @return the bus-specific ioctl return value + */ + virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * PROM CRC routine ported from MS5611 application note + * + * @param n_prom Pointer to words read from PROM. + * @return True if the CRC matches. + */ + bool crc4(uint16_t *n_prom); + +private: + /** * Test whether the device supported by the driver is present at a * specific address. @@ -157,8 +240,56 @@ private: */ int probe_address(uint8_t address); +}; + +class MS5611_SPI : public device::SPI +{ +public: + MS5611_SPI(int bus, const char* path, spi_dev_e device); + virtual ~MS5611_SPI(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + union ms5611_prom_u _prom; + + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct baro_report *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in kPa */ + + orb_advert_t _baro_topic; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + /** - * Initialise the automatic measurement state machine and start it. + * Initialize the automatic measurement state machine and start it. * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. @@ -198,26 +329,33 @@ private: * * @return OK if the measurement command was successful. */ - int measure(); + virtual int measure(); /** * Collect the result of the most recent measurement. */ - int collect(); + virtual int collect(); /** * Send a reset command to the MS5611. * * This is required after any bus reset. */ - int cmd_reset(); + virtual int cmd_reset(); /** * Read the MS5611 PROM * * @return OK if the PROM reads successfully. */ - int read_prom(); + virtual int read_prom(); + + /** + * Execute the bus-specific ioctl (I2C or SPI) + * + * @return the bus-specific ioctl return value + */ + virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); /** * PROM CRC routine ported from MS5611 application note @@ -227,6 +365,23 @@ private: */ bool crc4(uint16_t *n_prom); + // XXX this should really go into the SPI base class, as its common code + uint8_t read_reg(unsigned reg); + uint16_t read_reg16(unsigned reg); + void write_reg(unsigned reg, uint8_t value); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + +private: + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + }; /* helper macro for handling report buffer indices */ @@ -243,8 +398,13 @@ private: #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BUS PX4_I2C_BUS_ONBOARD -#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ #define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ @@ -259,8 +419,7 @@ private: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - -MS5611::MS5611(int bus) : +MS5611_I2C::MS5611_I2C(int bus) : I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), _measure_ticks(0), _num_reports(0), @@ -279,14 +438,46 @@ MS5611::MS5611(int bus) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // enable debug() calls - _debug_enabled = true; + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); + warnx("MS5611 I2C constructor"); +} +MS5611_SPI::MS5611_SPI(int bus, const char* path, spi_dev_e device) : + SPI("MS5611", path, bus, device, SPIDEV_MODE3, 2000000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) +{ // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + warnx("MS5611 SPI constructor"); +} + +MS5611_I2C::~MS5611_I2C() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; } -MS5611::~MS5611() +MS5611_SPI::~MS5611_SPI() { /* make sure we are truly inactive */ stop_cycle(); @@ -297,7 +488,7 @@ MS5611::~MS5611() } int -MS5611::init() +MS5611_I2C::init() { int ret = ERROR; @@ -327,8 +518,9 @@ out: } int -MS5611::probe() +MS5611_I2C::probe() { +#ifdef PX4_I2C_OBDEV_MS5611 _retries = 10; if ((OK == probe_address(MS5611_ADDRESS_1)) || @@ -340,12 +532,64 @@ MS5611::probe() _retries = 0; return OK; } +#endif return -EIO; } int -MS5611::probe_address(uint8_t address) +MS5611_SPI::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + warnx("MS5611 SPI init function"); + if (SPI::init() != OK) { + warnx("SPI init failed"); + goto out; + } else { + warnx("SPI init ok"); + } + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct baro_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the baro topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + + if (_baro_topic < 0) + debug("failed to create sensor_baro object"); + + ret = OK; +out: + return ret; +} + +int +MS5611_SPI::probe() +{ + + warnx("SPI probe"); + /* send reset command */ + if (OK != cmd_reset()) + return -EIO; + + /* read PROM */ + if (OK != read_prom()) + return -EIO; + + return OK; +} + +int +MS5611_I2C::probe_address(uint8_t address) { /* select the address we are going to try */ set_address(address); @@ -362,7 +606,7 @@ MS5611::probe_address(uint8_t address) } ssize_t -MS5611::read(struct file *filp, char *buffer, size_t buflen) +MS5611_I2C::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); int ret = 0; @@ -432,34 +676,238 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int -MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) +ssize_t +MS5611_SPI::read(struct file *filp, char *buffer, size_t buflen) { - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { + unsigned count = buflen / sizeof(struct baro_report); + int ret = 0; - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; - return OK; + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { - /* zero would be bad */ - case 0: - return -EINVAL; + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } - /* set interval for next measurement to minimum legal value */ + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MS5611_SPI::bus_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); +} + +int +MS5611_I2C::bus_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); +} + +int +MS5611_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return I2C::ioctl(filp, cmd, arg); +} + +int +MS5611_SPI::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ @@ -507,145 +955,498 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 2) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return SPI::ioctl(filp, cmd, arg); +} + +void +MS5611_I2C::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611_I2C::cycle_trampoline, this, 1); +} + +void +MS5611_I2C::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611_I2C::cycle_trampoline(void *arg) +{ + MS5611_I2C *dev = (MS5611_I2C *)arg; + + dev->cycle(); +} + +void +MS5611_I2C::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_I2C::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_I2C::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +void +MS5611_SPI::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611_SPI::cycle_trampoline, this, 1); +} + +void +MS5611_SPI::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611_SPI::cycle_trampoline(void *arg) +{ + MS5611_SPI *dev = (MS5611_SPI *)arg; + + dev->cycle(); +} + +void +MS5611_SPI::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with the message. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_SPI::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611_SPI::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +int +MS5611_I2C::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + * + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the write. + */ + _retries = 0; + ret = transfer(&cmd_data, 1, nullptr, 0); + + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +MS5611_I2C::collect() +{ + int ret; + uint8_t cmd; + uint8_t data[3]; + union { + uint8_t b[4]; + uint32_t w; + } cvt; + + /* read the most recent measurement */ + cmd = 0; + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + + ret = transfer(&cmd, 1, &data[0], 3); + if (ret != OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* fetch the raw value */ + cvt.b[0] = data[2]; + cvt.b[1] = data[1]; + cvt.b[2] = data[0]; + cvt.b[3] = 0; + uint32_t raw = cvt.w; + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + + /* generate a new report */ + _reports[_next_report].temperature = _TEMP / 100.0f; + _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); - if (nullptr == buf) - return -ENOMEM; + perf_end(_sample_perf); - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); + return OK; +} - return OK; - } +int +MS5611_I2C::cmd_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + return result; +} - case BAROIOCSMSLPRESSURE: +int +MS5611_I2C::read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) - return -EINVAL; + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); - _msl_pressure = arg; - return OK; + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - case BAROIOCGMSLPRESSURE: - return _msl_pressure; + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; - default: - break; + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom.c[i] = cvt.w; } - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + /* calculate CRC and return success/failure accordingly */ + return crc4(&_prom.c[0]) ? OK : -EIO; } -void -MS5611::start_cycle() +uint8_t +MS5611_SPI::read_reg(unsigned reg) { + uint8_t cmd[2]; - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _oldest_report = _next_report = 0; + cmd[0] = reg | DIR_READ; - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); -} + transfer(cmd, cmd, sizeof(cmd)); -void -MS5611::stop_cycle() -{ - work_cancel(HPWORK, &_work); + return cmd[1]; } -void -MS5611::cycle_trampoline(void *arg) +uint16_t +MS5611_SPI::read_reg16(unsigned reg) { - MS5611 *dev = (MS5611 *)arg; + uint8_t cmd[3]; - dev->cycle(); + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; } void -MS5611::cycle() +MS5611_SPI::write_reg(unsigned reg, uint8_t value) { - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with the message. - */ - } else { - //log("collection error %d", ret); - } - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + uint8_t cmd[2]; - return; - } - } + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; - /* measurement phase */ - ret = measure(); - if (ret != OK) { - //log("measure error %d", ret); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } + transfer(cmd, nullptr, sizeof(cmd)); +} - /* next phase is collection */ - _collect_phase = true; +void +MS5611_SPI::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); } int -MS5611::measure() +MS5611_SPI::measure() { int ret; @@ -655,6 +1456,7 @@ MS5611::measure() * In phase zero, request temperature; in other phases, request pressure. */ uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + cmd_data |= DIR_WRITE; /* * Send the command to begin measuring. @@ -662,8 +1464,7 @@ MS5611::measure() * Disable retries on this command; we can't know whether failure * means the device did or did not see the write. */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); + ret = transfer(&cmd_data, nullptr, 1); if (OK != ret) perf_count(_comms_errors); @@ -674,25 +1475,25 @@ MS5611::measure() } int -MS5611::collect() +MS5611_SPI::collect() { int ret; - uint8_t cmd; - uint8_t data[3]; + + uint8_t data[4]; union { uint8_t b[4]; uint32_t w; } cvt; /* read the most recent measurement */ - cmd = 0; + data[0] = 0 | DIR_WRITE; perf_begin(_sample_perf); /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - ret = transfer(&cmd, 1, &data[0], 3); + ret = transfer(&data[0], &data[0], sizeof(data)); if (ret != OK) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -700,9 +1501,9 @@ MS5611::collect() } /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; + cvt.b[0] = data[3]; + cvt.b[1] = data[2]; + cvt.b[2] = data[1]; cvt.b[3] = 0; uint32_t raw = cvt.w; @@ -761,30 +1562,7 @@ MS5611::collect() * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us */ -#if 0/* USE_FLOAT */ - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ - const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - float p1 = _msl_pressure / 1000.0f; - - /* measured pressure in kPa */ - float p = P / 1000.0f; - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#else /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ @@ -807,7 +1585,7 @@ MS5611::collect() * a */ _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#endif + /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); @@ -833,24 +1611,21 @@ MS5611::collect() } int -MS5611::cmd_reset() +MS5611_SPI::cmd_reset() { - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; int result; - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; + result = transfer(&cmd, nullptr, 1); + warnx("transferred reset, result: %d", result); return result; } int -MS5611::read_prom() +MS5611_SPI::read_prom() { - uint8_t prom_buf[2]; + uint8_t prom_buf[3]; union { uint8_t b[2]; uint16_t w; @@ -864,23 +1639,91 @@ MS5611::read_prom() /* read and convert PROM words */ for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); + _prom.c[i] = read_reg16(cmd); + } - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; + warnx("going for CRC"); - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; + /* calculate CRC and return success/failure accordingly */ + int ret = crc4(&_prom.c[0]) ? OK : -EIO; + if (ret == OK) { + warnx("CRC OK"); + } else { + warnx("CRC FAIL"); } + return ret; +} - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; +bool +MS5611_I2C::crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + +void +MS5611_I2C::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.s.factory_setup); + printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.s.serial_and_crc); } bool -MS5611::crc4(uint16_t *n_prom) +MS5611_SPI::crc4(uint16_t *n_prom) { int16_t cnt; uint16_t n_rem; @@ -923,7 +1766,7 @@ MS5611::crc4(uint16_t *n_prom) } void -MS5611::print_info() +MS5611_SPI::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -952,7 +1795,7 @@ MS5611::print_info() namespace ms5611 { -MS5611 *g_dev; +device::CDev *g_dev; void start(); void test(); @@ -971,8 +1814,21 @@ start() if (g_dev != nullptr) errx(1, "already started"); - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); + /* create the driver, try SPI first, fall back to I2C if required */ + #ifdef PX4_SPIDEV_BARO + { + warnx("Attempting SPI start"); + g_dev = new MS5611_SPI(1 /* XXX magic number, SPI1 */, BARO_DEVICE_PATH,(spi_dev_e)PX4_SPIDEV_BARO); + } + #endif + /* try I2C if configuration exists and SPI failed*/ + #ifdef MS5611_BUS + if (g_dev == nullptr) { + warnx("Attempting I2C start"); + g_dev = new MS5611_I2C(MS5611_BUS); + } + + #endif if (g_dev == nullptr) goto fail; @@ -1096,7 +1952,8 @@ info() errx(1, "driver not running"); printf("state @ %p\n", g_dev); - g_dev->print_info(); + MS5611_SPI* spi = (MS5611_SPI*)g_dev; + spi->print_info(); exit(0); } @@ -1154,11 +2011,11 @@ calibrate(unsigned altitude) const float g = 9.80665f; /* gravity constant in m/s/s */ const float R = 287.05f; /* ideal gas constant in J/kg/K */ - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - warnx("calculated MSL pressure %10.4fkPa", p1); + warnx("calculated MSL pressure %10.4fkPa", (double)p1); /* save as integer Pa */ p1 *= 1000.0f; @@ -1211,4 +2068,4 @@ ms5611_main(int argc, char *argv[]) } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -} +} \ No newline at end of file -- cgit v1.2.3 From 90c458cb618754905ab6d373f22d76e3309adf4c Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 25 Jun 2013 23:08:34 -0700 Subject: Checkpoint: interface abstraction for px4io driver --- makefiles/config_px4fmuv2_default.mk | 1 + src/drivers/px4io/interface.h | 78 +++++++ src/drivers/px4io/interface_i2c.cpp | 179 ++++++++++++++++ src/drivers/px4io/interface_serial.cpp | 361 +++++++++++++++++++++++++++++++++ src/drivers/px4io/module.mk | 7 +- src/drivers/px4io/px4io.cpp | 93 ++++----- src/modules/px4iofirmware/protocol.h | 44 +--- src/modules/systemlib/hx_stream.c | 213 +++++++++++-------- src/modules/systemlib/hx_stream.h | 60 ++++-- 9 files changed, 845 insertions(+), 191 deletions(-) create mode 100644 src/drivers/px4io/interface.h create mode 100644 src/drivers/px4io/interface_i2c.cpp create mode 100644 src/drivers/px4io/interface_serial.cpp diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk index 26c249901..0463ccd84 100644 --- a/makefiles/config_px4fmuv2_default.mk +++ b/makefiles/config_px4fmuv2_default.mk @@ -15,6 +15,7 @@ MODULES += drivers/stm32 MODULES += drivers/stm32/adc MODULES += drivers/stm32/tone_alarm MODULES += drivers/px4fmu +MODULES += drivers/px4io MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h new file mode 100644 index 000000000..834cb9e07 --- /dev/null +++ b/src/drivers/px4io/interface.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 interface.h + * + * PX4IO interface classes. + */ + +#include + +#include + +class PX4IO_interface +{ +public: + /** + * Check that the interface initialised OK. + * + * Does not check that communication has been established. + */ + virtual bool ok() = 0; + + /** + * Set PX4IO registers. + * + * @param page The register page to write + * @param offset Offset of the first register to write + * @param values Pointer to values to write + * @param num_values The number of values to write + * @return Zero on success. + */ + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) = 0; + + /** + * Get PX4IO registers. + * + * @param page The register page to read + * @param offset Offset of the first register to read + * @param values Pointer to store values read + * @param num_values The number of values to read + * @return Zero on success. + */ + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0; +}; + +extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address); +extern PX4IO_interface *io_serial_interface(int port); diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp new file mode 100644 index 000000000..6895a7e23 --- /dev/null +++ b/src/drivers/px4io/interface_i2c.cpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * 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 interface_i2c.cpp + * + * I2C interface for PX4IO + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include "uploader.h" +#include + +#include "interface.h" + +class PX4IO_I2C : public PX4IO_interface +{ +public: + PX4IO_I2C(int bus, uint8_t address); + virtual ~PX4IO_I2C(); + + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + virtual bool ok(); + +private: + static const unsigned _retries = 2; + + struct i2c_dev_s *_dev; + uint8_t _address; +}; + +PX4IO_interface *io_i2c_interface(int bus, uint8_t address) +{ + return new PX4IO_I2C(bus, address); +} + +PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : + _dev(nullptr), + _address(address) +{ + _dev = up_i2cinitialize(bus); + if (_dev) + I2C_SETFREQUENCY(_dev, 400000); +} + +PX4IO_I2C::~PX4IO_I2C() +{ + if (_dev) + up_i2cuninitialize(_dev); +} + +bool +PX4IO_I2C::ok() +{ + if (!_dev) + return false; + + /* check any other status here */ + + return true; +} + +int +PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + int ret; + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].addr = _address; + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].addr = _address; + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + unsigned tries = 0; + do { + + /* perform the transfer */ + ret = I2C_TRANSFER(_dev, msgv, 2); + + if (ret == OK) + break; + + } while (tries++ < _retries); + + return ret; +} + +int +PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + int ret; + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].addr = _address; + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].addr = _address; + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + unsigned tries = 0; + do { + /* perform the transfer */ + ret = I2C_TRANSFER(_dev, msgv, 2); + + if (ret == OK) + break; + + } while (tries++ < _retries); + + return ret; +} diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp new file mode 100644 index 000000000..f91284c72 --- /dev/null +++ b/src/drivers/px4io/interface_serial.cpp @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * 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 interface_serial.cpp + * + * Serial interface for PX4IO + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/* XXX might be able to prune these */ +#include +#include +#include +#include + +#include + +#include + +#include "interface.h" + +class PX4IO_serial : public PX4IO_interface +{ +public: + PX4IO_serial(int port); + virtual ~PX4IO_serial(); + + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + virtual bool ok(); + +private: + volatile uint32_t *_serial_base; + int _vector; + + uint8_t *_tx_buf; + unsigned _tx_size; + + const uint8_t *_rx_buf; + unsigned _rx_size; + + hx_stream_t _stream; + + sem_t _bus_semaphore; + sem_t _completion_semaphore; + + /** + * Send _tx_size bytes from the buffer, then + * if _rx_size is greater than zero wait for a packet + * to come back. + */ + int _wait_complete(); + + /** + * Interrupt handler. + */ + static int _interrupt(int irq, void *context); + void _do_interrupt(); + + /** + * Stream transmit callback + */ + static void _tx(void *arg, uint8_t data); + void _do_tx(uint8_t data); + + /** + * Stream receive callback + */ + static void _rx(void *arg, const void *data, size_t length); + void _do_rx(const uint8_t *data, size_t length); + + /** + * Serial register accessors. + */ + volatile uint32_t &_sreg(unsigned offset) + { + return *(_serial_base + (offset / sizeof(uint32_t))); + } + volatile uint32_t &_SR() { return _sreg(STM32_USART_SR_OFFSET); } + volatile uint32_t &_DR() { return _sreg(STM32_USART_DR_OFFSET); } + volatile uint32_t &_BRR() { return _sreg(STM32_USART_BRR_OFFSET); } + volatile uint32_t &_CR1() { return _sreg(STM32_USART_CR1_OFFSET); } + volatile uint32_t &_CR2() { return _sreg(STM32_USART_CR2_OFFSET); } + volatile uint32_t &_CR3() { return _sreg(STM32_USART_CR3_OFFSET); } + volatile uint32_t &_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } +}; + +/* XXX hack to avoid expensive IRQ lookup */ +static PX4IO_serial *io_serial; + +PX4IO_interface *io_serial_interface(int port) +{ + return new PX4IO_serial(port); +} + +PX4IO_serial::PX4IO_serial(int port) : + _serial_base(0), + _vector(0), + _tx_buf(nullptr), + _tx_size(0), + _rx_size(0), + _stream(0) +{ + /* only allow one instance */ + if (io_serial != nullptr) + return; + + switch (port) { + case 5: + _serial_base = (volatile uint32_t *)STM32_UART5_BASE; + _vector = STM32_IRQ_UART5; + break; + default: + /* not a supported port */ + return; + } + + /* need space for worst-case escapes + hx protocol overhead */ + /* XXX this is kinda gross, but hx transmits a byte at a time */ + _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; + + irq_attach(_vector, &_interrupt); + + _stream = hx_stream_init(-1, _rx, this); + + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); +} + +PX4IO_serial::~PX4IO_serial() +{ + + if (_tx_buf != nullptr) + delete[] _tx_buf; + + if (_vector) + irq_detach(_vector); + + if (io_serial == this) + io_serial = nullptr; + + if (_stream) + hx_stream_free(_stream); + + sem_destroy(&_completion_semaphore); + sem_destroy(&_bus_semaphore); +} + +bool +PX4IO_serial::ok() +{ + if (_serial_base == 0) + return false; + if (_vector == 0) + return false; + if (_tx_buf == nullptr) + return false; + if (!_stream) + return false; + + return true; +} + +int +PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + + unsigned count = num_values * sizeof(*values); + if (count > (HX_STREAM_MAX_FRAME - 2)) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + _tx_buf[0] = page; + _tx_buf[1] = offset; + memcpy(&_tx_buf[2], (void *)values, count); + + _tx_size = count + 2; + _rx_size = 0; + + /* start the transaction and wait for it to complete */ + int result = _wait_complete(); + + sem_post(&_bus_semaphore); + return result; +} + +int +PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + + unsigned count = num_values * sizeof(*values); + if (count > HX_STREAM_MAX_FRAME) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + _tx_buf[0] = page; + _tx_buf[1] = offset; + _tx_buf[2] = num_values; + + _tx_size = 3; /* this tells IO that this is a read request */ + _rx_size = count; + + /* start the transaction and wait for it to complete */ + int result = _wait_complete(); + if (result != OK) + goto out; + + /* compare the received count with the expected count */ + if (_rx_size != count) { + return -EIO; + } else { + /* copy back the result */ + memcpy(values, &_tx_buf[0], count); + } +out: + sem_post(&_bus_semaphore); + return OK; +} + +int +PX4IO_serial::_wait_complete() +{ + /* prepare the stream for transmission */ + hx_stream_reset(_stream); + hx_stream_start(_stream, _tx_buf, _tx_size); + + /* enable UART */ + _CR1() |= USART_CR1_RE | + USART_CR1_TE | + USART_CR1_TXEIE | + USART_CR1_RXNEIE | + USART_CR1_UE; + + /* compute the deadline for a 5ms timeout */ + struct timespec abstime; + clock_gettime(CLOCK_REALTIME, &abstime); + abstime.tv_nsec += 5000000; /* 5ms timeout */ + while (abstime.tv_nsec > 1000000000) { + abstime.tv_sec++; + abstime.tv_nsec -= 1000000000; + } + + /* wait for the transaction to complete */ + int ret = sem_timedwait(&_completion_semaphore, &abstime); + + /* disable the UART */ + _CR1() &= ~(USART_CR1_RE | + USART_CR1_TE | + USART_CR1_TXEIE | + USART_CR1_RXNEIE | + USART_CR1_UE); + + return ret; +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + /* ... because NuttX doesn't give us a handle per vector */ + io_serial->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = _SR(); + + /* handle transmit completion */ + if (sr & USART_SR_TXE) { + int c = hx_stream_send_next(_stream); + if (c == -1) { + /* transmit (nearly) done, not interested in TX-ready interrupts now */ + _CR1() &= ~USART_CR1_TXEIE; + + /* was this a tx-only operation? */ + if (_rx_size == 0) { + /* wake up waiting sender */ + sem_post(&_completion_semaphore); + } + } else { + _DR() = c; + } + } + + if (sr & USART_SR_RXNE) { + uint8_t c = _DR(); + + hx_stream_rx(_stream, c); + } +} + +void +PX4IO_serial::_rx(void *arg, const void *data, size_t length) +{ + PX4IO_serial *pserial = reinterpret_cast(arg); + + pserial->_do_rx((const uint8_t *)data, length); +} + +void +PX4IO_serial::_do_rx(const uint8_t *data, size_t length) +{ + _rx_buf = data; + + if (length < _rx_size) + _rx_size = length; + + /* notify waiting receiver */ + sem_post(&_completion_semaphore); +} + + + diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 328e5a684..d5bab6599 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -38,4 +38,9 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ - uploader.cpp + uploader.cpp \ + interface_serial.cpp \ + interface_i2c.cpp + +# XXX prune to just get UART registers +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe..ecf50c859 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -58,7 +58,6 @@ #include #include -#include #include #include #include @@ -83,16 +82,18 @@ #include #include -#include "uploader.h" #include +#include "uploader.h" +#include "interface.h" + #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -class PX4IO : public device::I2C +class PX4IO : public device::CDev { public: - PX4IO(); + PX4IO(PX4IO_interface *interface); virtual ~PX4IO(); virtual int init(); @@ -130,6 +131,8 @@ public: void print_status(); private: + PX4IO_interface *_interface; + // XXX unsigned _max_actuators; unsigned _max_controls; @@ -312,8 +315,9 @@ PX4IO *g_dev; } -PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), +PX4IO::PX4IO(PX4IO_interface *interface) : + CDev("px4io", "/dev/px4io"), + _interface(interface), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -364,6 +368,9 @@ PX4IO::~PX4IO() if (_task != -1) task_delete(_task); + if (_interface != nullptr) + delete _interface; + g_dev = nullptr; } @@ -375,18 +382,10 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = I2C::init(); + ret = CDev::init(); if (ret != OK) return ret; - /* - * Enable a couple of retries for operations to IO. - * - * Register read/write operations are intentionally idempotent - * so this is safe as designed. - */ - _retries = 2; - /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); @@ -395,7 +394,7 @@ PX4IO::init() _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays < 1) || (_max_relays > 255) || - (_max_transfer < 16) || (_max_transfer > 255) || + (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); @@ -700,8 +699,6 @@ PX4IO::io_set_control_state() int PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; - if (len > _max_actuators) /* fail with error */ return E2BIG; @@ -1114,22 +1111,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_NORESTART; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - /* perform the transfer */ - int ret = transfer(msgv, 2); + int ret = _interface->set_reg(page, offset, values, num_values); if (ret != OK) debug("io_reg_set: error %d", ret); return ret; @@ -1144,22 +1126,13 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - /* perform the transfer */ - int ret = transfer(msgv, 2); + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + return -EINVAL; + } + + int ret = _interface->get_reg(page, offset, values, num_values); if (ret != OK) debug("io_reg_get: data error %d", ret); return ret; @@ -1603,8 +1576,26 @@ start(int argc, char *argv[]) if (g_dev != nullptr) errx(1, "already loaded"); + PX4IO_interface *interface; + +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + interface = io_serial_interface(5); /* XXX wrong port! */ +#elif defined(CONFIG_ARCH_BOARD_PX4FMU) + interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +#else +# error Unknown board - cannot select interface. +#endif + + if (interface == nullptr) + errx(1, "cannot alloc interface"); + + if (!interface->ok()) { + delete interface; + errx(1, "interface init failed"); + } + /* create the driver - it will set g_dev */ - (void)new PX4IO(); + (void)new PX4IO(interface); if (g_dev == nullptr) errx(1, "driver alloc failed"); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 90d63ea1a..0e40bff69 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,8 +36,7 @@ /** * @file protocol.h * - * PX4IO interface protocol - * ======================== + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -46,7 +45,7 @@ * respectively. Subsequent reads and writes increment the offset within * the page. * - * Most pages are readable or writable but not both. + * Some pages are read- or write-only. * * Note that some pages may permit offset values greater than 255, which * can only be achieved by long writes. The offset does not wrap. @@ -63,29 +62,6 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. - * - * PX4IO I2C interface notes - * ------------------------- - * - * Register read/write operations are mapped directly to PX4IO register - * read/write operations. - * - * PX4IO Serial interface notes - * ---------------------------- - * - * The MSB of the page number is used to distinguish between read and - * write operations. If set, the operation is a write and additional - * data is expected to follow in the packet as for I2C writes. - * - * If clear, the packet is expected to contain a single byte giving the - * number of registers to be read. PX4IO will respond with a packet containing - * the same header (page, offset) and the requested data. - * - * If a read is requested when PX4IO does not have buffer space to store - * the reply, the request will be dropped. PX4IO is always configured with - * enough space to receive one full-sized write and one read request, and - * to send one full-sized read response. - * */ #define PX4IO_CONTROL_CHANNELS 8 @@ -99,14 +75,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) -#define PX4IO_PAGE_WRITE (1<<7) - /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ -#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum packet transfer size */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ @@ -168,7 +142,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 64 +#define PX4IO_PAGE_SETUP 100 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -186,13 +160,13 @@ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 65 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 66 /* see px4io_mixdata structure below */ +#define PX4IO_PAGE_MIXERLOAD 102 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 67 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -204,10 +178,10 @@ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 68 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 69 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 88f7f762c..fdc3edac7 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -53,14 +53,26 @@ struct hx_stream { - uint8_t buf[HX_STREAM_MAX_FRAME + 4]; - unsigned frame_bytes; - bool escaped; - bool txerror; - + /* RX state */ + uint8_t rx_buf[HX_STREAM_MAX_FRAME + 4]; + unsigned rx_frame_bytes; + bool rx_escaped; + hx_stream_rx_callback rx_callback; + void *rx_callback_arg; + + /* TX state */ int fd; - hx_stream_rx_callback callback; - void *callback_arg; + bool tx_error; + uint8_t *tx_buf; + unsigned tx_resid; + uint32_t tx_crc; + enum { + TX_IDLE = 0, + TX_SEND_START, + TX_SEND_DATA, + TX_SENT_ESCAPE, + TX_SEND_END + } tx_state; perf_counter_t pc_tx_frames; perf_counter_t pc_rx_frames; @@ -76,13 +88,12 @@ struct hx_stream { static void hx_tx_raw(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c); static int hx_rx_frame(hx_stream_t stream); -static bool hx_rx_char(hx_stream_t stream, uint8_t c); static void hx_tx_raw(hx_stream_t stream, uint8_t c) { if (write(stream->fd, &c, 1) != 1) - stream->txerror = true; + stream->tx_error = true; } static void @@ -106,11 +117,11 @@ hx_rx_frame(hx_stream_t stream) uint8_t b[4]; uint32_t w; } u; - unsigned length = stream->frame_bytes; + unsigned length = stream->rx_frame_bytes; /* reset the stream */ - stream->frame_bytes = 0; - stream->escaped = false; + stream->rx_frame_bytes = 0; + stream->rx_escaped = false; /* not a real frame - too short */ if (length < 4) { @@ -123,11 +134,11 @@ hx_rx_frame(hx_stream_t stream) length -= 4; /* compute expected CRC */ - u.w = crc32(&stream->buf[0], length); + u.w = crc32(&stream->rx_buf[0], length); /* compare computed and actual CRC */ for (unsigned i = 0; i < 4; i++) { - if (u.b[i] != stream->buf[length + i]) { + if (u.b[i] != stream->rx_buf[length + i]) { perf_count(stream->pc_rx_errors); return 0; } @@ -135,7 +146,7 @@ hx_rx_frame(hx_stream_t stream) /* frame is good */ perf_count(stream->pc_rx_frames); - stream->callback(stream->callback_arg, &stream->buf[0], length); + stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length); return 1; } @@ -151,8 +162,8 @@ hx_stream_init(int fd, if (stream != NULL) { memset(stream, 0, sizeof(struct hx_stream)); stream->fd = fd; - stream->callback = callback; - stream->callback_arg = arg; + stream->rx_callback = callback; + stream->rx_callback_arg = arg; } return stream; @@ -180,105 +191,135 @@ hx_stream_set_counters(hx_stream_t stream, stream->pc_rx_errors = rx_errors; } +void +hx_stream_reset(hx_stream_t stream) +{ + stream->rx_frame_bytes = 0; + stream->rx_escaped = false; + + stream->tx_buf = NULL; + stream->tx_resid = 0; + stream->tx_state = TX_IDLE; +} + int -hx_stream_send(hx_stream_t stream, +hx_stream_start(hx_stream_t stream, const void *data, size_t count) { - union { - uint8_t b[4]; - uint32_t w; - } u; - const uint8_t *p = (const uint8_t *)data; - unsigned resid = count; - - if (resid > HX_STREAM_MAX_FRAME) + if (count > HX_STREAM_MAX_FRAME) return -EINVAL; - /* start the frame */ - hx_tx_raw(stream, FBO); + stream->tx_buf = data; + stream->tx_resid = count; + stream->tx_state = TX_SEND_START; + stream->tx_crc = crc32(data, count); + return OK; +} + +int +hx_stream_send_next(hx_stream_t stream) +{ + int c; - /* transmit the data */ - while (resid--) - hx_tx_byte(stream, *p++); + /* sort out what we're going to send */ + switch (stream->tx_state) { - /* compute the CRC */ - u.w = crc32(data, count); + case TX_SEND_START: + stream->tx_state = TX_SEND_DATA; + return FBO; - /* send the CRC */ - p = &u.b[0]; - resid = 4; + case TX_SEND_DATA: + c = *stream->tx_buf; - while (resid--) - hx_tx_byte(stream, *p++); + switch (c) { + case FBO: + case CEO: + stream->tx_state = TX_SENT_ESCAPE; + return CEO; + } + break; + + case TX_SENT_ESCAPE: + c = *stream->tx_buf ^ 0x20; + stream->tx_state = TX_SEND_DATA; + break; - /* and the trailing frame separator */ - hx_tx_raw(stream, FBO); + case TX_SEND_END: + stream->tx_state = TX_IDLE; + return FBO; + + case TX_IDLE: + default: + return -1; + } + + /* if we are here, we have consumed a byte from the buffer */ + stream->tx_resid--; + stream->tx_buf++; + + /* buffer exhausted */ + if (stream->tx_resid == 0) { + uint8_t *pcrc = (uint8_t *)&stream->tx_crc; + + /* was the buffer the frame CRC? */ + if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) { + stream->tx_state = TX_SEND_END; + } else { + /* no, it was the payload - switch to sending the CRC */ + stream->tx_buf = pcrc; + stream->tx_resid = sizeof(stream->tx_crc); + } + } + return c; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + int result; + + result = hx_start(stream, data, count); + if (result != OK) + return result; + + int c; + while ((c = hx_send_next(stream)) >= 0) + hx_tx_raw(stream, c); /* check for transmit error */ - if (stream->txerror) { - stream->txerror = false; + if (stream->tx_error) { + stream->tx_error = false; return -EIO; } perf_count(stream->pc_tx_frames); - return 0; + return OK; } -static bool -hx_rx_char(hx_stream_t stream, uint8_t c) +void +hx_stream_rx(hx_stream_t stream, uint8_t c) { /* frame end? */ if (c == FBO) { hx_rx_frame(stream); - return true; + return; } /* escaped? */ - if (stream->escaped) { - stream->escaped = false; + if (stream->rx_escaped) { + stream->rx_escaped = false; c ^= 0x20; } else if (c == CEO) { - /* now escaped, ignore the byte */ - stream->escaped = true; - return false; + /* now rx_escaped, ignore the byte */ + stream->rx_escaped = true; + return; } /* save for later */ - if (stream->frame_bytes < sizeof(stream->buf)) - stream->buf[stream->frame_bytes++] = c; - - return false; -} - -void -hx_stream_rx_char(hx_stream_t stream, uint8_t c) -{ - hx_rx_char(stream, c); -} - -int -hx_stream_rx(hx_stream_t stream) -{ - uint16_t buf[16]; - ssize_t len; - - /* read bytes */ - len = read(stream->fd, buf, sizeof(buf)); - if (len <= 0) { - - /* nonblocking stream and no data */ - if (errno == EWOULDBLOCK) - return 0; - - /* error or EOF */ - return -errno; - } - - /* process received characters */ - for (int i = 0; i < len; i++) - hx_rx_char(stream, buf[i]); - - return 0; + if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) + stream->rx_buf[stream->rx_frame_bytes++] = c; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index be4850f74..1f3927222 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -58,7 +58,8 @@ __BEGIN_DECLS * Allocate a new hx_stream object. * * @param fd The file handle over which the protocol will - * communicate. + * communicate, or -1 if the protocol will use + * hx_stream_start/hx_stream_send_next. * @param callback Called when a frame is received. * @param callback_arg Passed to the callback. * @return A handle to the stream, or NULL if memory could @@ -80,6 +81,7 @@ __EXPORT extern void hx_stream_free(hx_stream_t stream); * * Any counter may be set to NULL to disable counting that datum. * + * @param stream A handle returned from hx_stream_init. * @param tx_frames Counter for transmitted frames. * @param rx_frames Counter for received frames. * @param rx_errors Counter for short and corrupt received frames. @@ -89,6 +91,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream, perf_counter_t rx_frames, perf_counter_t rx_errors); +/** + * Reset a stream. + * + * Forces the local stream state to idle. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_reset(hx_stream_t stream); + +/** + * Prepare to send a frame. + * + * Use this in conjunction with hx_stream_send_next to + * set the frame to be transmitted. + * + * Use hx_stream_send() to write to the stream fd directly. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, -errno on error. + */ +__EXPORT extern int hx_stream_start(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Get the next byte to send for a stream. + * + * This requires that the stream be prepared for sending by + * calling hx_stream_start first. + * + * @param stream A handle returned from hx_stream_init. + * @return The byte to send, or -1 if there is + * nothing left to send. + */ +__EXPORT extern int hx_stream_send_next(hx_stream_t stream); + /** * Send a frame. * @@ -114,25 +154,9 @@ __EXPORT extern int hx_stream_send(hx_stream_t stream, * @param stream A handle returned from hx_stream_init. * @param c The character to process. */ -__EXPORT extern void hx_stream_rx_char(hx_stream_t stream, +__EXPORT extern void hx_stream_rx(hx_stream_t stream, uint8_t c); -/** - * Handle received bytes from the stream. - * - * Note that this interface should only be used with blocking streams - * when it is OK for the call to block until a frame is received. - * - * When used with a non-blocking stream, it will typically return - * immediately, or after handling a received frame. - * - * @param stream A handle returned from hx_stream_init. - * @return -errno on error, nonzero if a frame - * has been received, or if not enough - * bytes are available to complete a frame. - */ -__EXPORT extern int hx_stream_rx(hx_stream_t stream); - __END_DECLS #endif -- cgit v1.2.3 From 76346bfe19c816491a6982abfa10f48cd9d258f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Jun 2013 20:20:27 +0200 Subject: Corrected merge mistake --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index bdb98a24e..8a07855c0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -686,7 +686,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; - struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; -- cgit v1.2.3 From 1759f30e3aa4b2da25dcd0c836480f069d917a88 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 11:10:46 +0400 Subject: Sonar added to position_estimator_inav --- .../position_estimator_inav/inertial_filter.c | 7 +- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 79 +++++++++++++++++----- .../position_estimator_inav_params.c | 15 ++++ .../position_estimator_inav_params.h | 10 +++ 5 files changed, 93 insertions(+), 20 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index acaf693f1..13328edb4 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,9 +13,12 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float edt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * edt; + float ewdt = w * dt; + if (ewdt > 1.0f) + ewdt = 1.0f; // prevent over-correcting + ewdt *= e; x[i] += ewdt; if (i == 0) { diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index dca6375dc..761c17097 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -10,4 +10,4 @@ void inertial_filter_predict(float dt, float x[3]); -void inertial_filter_correct(float edt, float x[3], int i, float w); +void inertial_filter_correct(float e, float dt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 278a319b5..fb09948ec 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -183,6 +184,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -191,6 +194,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ @@ -263,12 +267,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; uint16_t accel_updates = 0; - hrt_abstime accel_t = 0; uint16_t baro_updates = 0; - hrt_abstime baro_t = 0; - hrt_abstime gps_t = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; @@ -283,6 +285,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) }; + float sonar_corr = 0.0f; + float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; /* main loop */ struct pollfd fds[5] = { @@ -290,6 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = optical_flow_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; @@ -297,7 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("main loop started."); while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -355,13 +364,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0]; + baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } } - if (params.use_gps && fds[4].revents & POLLIN) { + /* optical flow */ + if (fds[4].revents & POLLIN) { + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { + if (flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (fabsf(sonar_corr) > params.sonar_err) { + // correction is too large: spike or new ground level? + if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + // spike detected, ignore + sonar_corr = 0.0f; + } else { + // new ground level + baro_alt0 += sonar_corr; + warnx("new home: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); + local_pos.home_alt = baro_alt0; + local_pos.home_timestamp = hrt_absolute_time(); + sonar_corr = 0.0f; + sonar_corr_filtered = 0.0; + } + } + } + } else { + sonar_corr = 0.0f; + } + flow_updates++; + } + + if (params.use_gps && fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { @@ -393,8 +434,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); + baro_alt0 += sonar_corr * params.w_alt_sonar * dt; + inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); + inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; @@ -404,15 +447,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est); /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } } @@ -421,17 +464,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; - printf( - "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + warnx( + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", accel_updates / updates_dt, baro_updates / updates_dt, gps_updates / updates_dt, - attitude_updates / updates_dt); + attitude_updates / updates_dt, + flow_updates / updates_dt); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; + flow_updates = 0; } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 49dc7f51f..eac2fc1ce 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,18 +43,28 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); + h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->flow_k = param_find("INAV_FLOW_K"); + h->sonar_filt = param_find("INAV_SONAR_FILT"); + h->sonar_err = param_find("INAV_SONAR_ERR"); return OK; } @@ -64,9 +74,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_alt_sonar, &(p->w_alt_sonar)); param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); + param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->flow_k, &(p->flow_k)); + param_get(h->sonar_filt, &(p->sonar_filt)); + param_get(h->sonar_err, &(p->sonar_err)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 870227fef..cca172b5d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,18 +44,28 @@ struct position_estimator_inav_params { int use_gps; float w_alt_baro; float w_alt_acc; + float w_alt_sonar; float w_pos_gps_p; float w_pos_gps_v; float w_pos_acc; + float w_pos_flow; + float flow_k; + float sonar_filt; + float sonar_err; }; struct position_estimator_inav_param_handles { param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; + param_t w_alt_sonar; param_t w_pos_gps_p; param_t w_pos_gps_v; param_t w_pos_acc; + param_t w_pos_flow; + param_t flow_k; + param_t sonar_filt; + param_t sonar_err; }; /** -- cgit v1.2.3 From 8e732fc52763a05739e4f13caf0dff84817839cd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 12:58:12 +0400 Subject: position_estimator_inav default parameters changed, some fixes --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 10 ++++++++++ .../position_estimator_inav/position_estimator_inav_main.c | 1 + .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 508879ae2..3a14d516a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -209,6 +209,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_y_integral = 0.0f; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + float home_alt = 0.0f; + hrt_abstime home_alt_t = 0; thread_running = true; @@ -288,6 +290,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_sp_speed_z = 0.0f; if (status.flag_control_manual_enabled) { + if (local_pos.home_timestamp != home_alt_t) { + if (home_alt_t != 0) { + /* home alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z -= local_pos.home_alt - home_alt; + } + home_alt_t = local_pos.home_timestamp; + home_alt = local_pos.home_alt; + } /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fb09948ec..428269e4a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -391,6 +391,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.home_alt = baro_alt0; local_pos.home_timestamp = hrt_absolute_time(); + z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index eac2fc1ce..c90c611a7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) -- cgit v1.2.3 From 7dfaed3ee7718c15731070f3e3bd54d725f72029 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 23:35:48 +0400 Subject: multirotor_pos_control: new ground level -> altitude setpoint correction fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3a14d516a..cb6afa1cb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -293,7 +293,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z -= local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.home_alt - home_alt; } home_alt_t = local_pos.home_timestamp; home_alt = local_pos.home_alt; -- cgit v1.2.3 From d1562f926f487d1ed05751d45a2516be8c192564 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 28 Jun 2013 23:39:35 -0700 Subject: More implementation for the serial side on IO; fix a couple of bugs on the FMU side. Still needs serial init and some more testing/config on the FMU side, but closer to being ready to test. --- src/drivers/boards/px4iov2/px4iov2_internal.h | 6 ++ src/drivers/px4io/interface_serial.cpp | 23 ++--- src/modules/px4iofirmware/px4io.h | 7 +- src/modules/px4iofirmware/registers.c | 9 ++ src/modules/px4iofirmware/serial.c | 125 ++++++++++++++++---------- src/modules/systemlib/hx_stream.c | 18 +--- 6 files changed, 108 insertions(+), 80 deletions(-) diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index c4c592fe4..282ed7548 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -56,6 +56,12 @@ ******************************************************************************/ /* Configuration **************************************************************/ +/****************************************************************************** + * Serial + ******************************************************************************/ +#define SERIAL_BASE STM32_USART1_BASE +#define SERIAL_VECTOR STM32_IRQ_USART1 + /****************************************************************************** * GPIOS ******************************************************************************/ diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index f91284c72..d0af2912a 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -160,6 +160,8 @@ PX4IO_serial::PX4IO_serial(int port) : return; } + /* XXX need to configure the port here */ + /* need space for worst-case escapes + hx protocol overhead */ /* XXX this is kinda gross, but hx transmits a byte at a time */ _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; @@ -257,7 +259,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n return -EIO; } else { /* copy back the result */ - memcpy(values, &_tx_buf[0], count); + memcpy(values, &_rx_buf[0], count); } out: sem_post(&_bus_semaphore); @@ -267,16 +269,12 @@ out: int PX4IO_serial::_wait_complete() { - /* prepare the stream for transmission */ + /* prepare the stream for transmission (also discards any received noise) */ hx_stream_reset(_stream); hx_stream_start(_stream, _tx_buf, _tx_size); - /* enable UART */ - _CR1() |= USART_CR1_RE | - USART_CR1_TE | - USART_CR1_TXEIE | - USART_CR1_RXNEIE | - USART_CR1_UE; + /* enable transmit-ready interrupt, which will start transmission */ + _CR1() |= USART_CR1_TXEIE; /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -290,13 +288,6 @@ PX4IO_serial::_wait_complete() /* wait for the transaction to complete */ int ret = sem_timedwait(&_completion_semaphore, &abstime); - /* disable the UART */ - _CR1() &= ~(USART_CR1_RE | - USART_CR1_TE | - USART_CR1_TXEIE | - USART_CR1_RXNEIE | - USART_CR1_UE); - return ret; } @@ -317,7 +308,7 @@ PX4IO_serial::_do_interrupt() if (sr & USART_SR_TXE) { int c = hx_stream_send_next(_stream); if (c == -1) { - /* transmit (nearly) done, not interested in TX-ready interrupts now */ + /* no more bytes to send, not interested in interrupts now */ _CR1() &= ~USART_CR1_TXEIE; /* was this a tx-only operation? */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 2077e6244..47bcb8ddf 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,7 +42,12 @@ #include #include -#include +#ifdef CONFIG_ARCH_BOARD_PX4IO +# include +#endif +#ifdef CONFIG_ARCH_BOARD_PX4IOV2 +# include +#endif #include "protocol.h" diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..42554456c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -44,6 +44,7 @@ #include #include +#include #include "px4io.h" #include "protocol.h" @@ -349,10 +350,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; +#ifdef POWER_RELAY1 POWER_RELAY1(value & (1 << 0) ? 1 : 0); +#endif +#ifdef POWER_RELAY2 POWER_RELAY2(value & (1 << 1) ? 1 : 0); +#endif +#ifdef POWER_ACC1 POWER_ACC1(value & (1 << 2) ? 1 : 0); +#endif +#ifdef POWER_ACC2 POWER_ACC2(value & (1 << 3) ? 1 : 0); +#endif break; case PX4IO_P_SETUP_SET_DEBUG: diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index bf9456e94..f691969c4 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -46,36 +46,40 @@ #include #include +/* XXX might be able to prune these */ +#include +#include +#include +#include #include //#define DEBUG #include "px4io.h" -static uint8_t tx_buf[66]; /* XXX hardcoded magic number */ - static hx_stream_t if_stream; +static volatile bool sending = false; +static int serial_interrupt(int vector, void *context); static void serial_callback(void *arg, const void *data, unsigned length); +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + void interface_init(void) { - int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK); - if (fd < 0) { - debug("serial fail"); - return; - } - - /* configure serial port - XXX increase port speed? */ - struct termios t; - tcgetattr(fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(fd, TCSANOW, &t); + /* XXX do serial port init here */ - /* allocate the HX stream we'll use for communication */ - if_stream = hx_stream_init(fd, serial_callback, NULL); + irq_attach(SERIAL_VECTOR, serial_interrupt); + if_stream = hx_stream_init(-1, serial_callback, NULL); /* XXX add stream stats counters? */ @@ -85,8 +89,31 @@ interface_init(void) void interface_tick() { - /* process incoming bytes */ - hx_stream_rx(if_stream); + /* XXX nothing interesting to do here */ +} + +static int +serial_interrupt(int vector, void *context) +{ + uint32_t sr = rSR; + + if (sr & USART_SR_TXE) { + int c = hx_stream_send_next(if_stream); + if (c == -1) { + /* no more bytes to send, not interested in interrupts now */ + rCR1 &= ~USART_CR1_TXEIE; + sending = false; + } else { + rDR = c; + } + } + + if (sr & USART_SR_RXNE) { + uint8_t c = rDR; + + hx_stream_rx(if_stream, c); + } + return 0; } static void @@ -98,36 +125,40 @@ serial_callback(void *arg, const void *data, unsigned length) if (length < 2) return; - /* it's a write operation, pass it to the register API */ - if (message[0] & PX4IO_PAGE_WRITE) { - registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1], - (const uint16_t *)&message[2], - (length - 2) / 2); - + /* we got a new request while we were still sending the last reply - not supported */ + if (sending) return; - } - /* it's a read - must contain length byte */ - if (length != 3) + /* reads are page / offset / length */ + if (length == 3) { + uint16_t *registers; + unsigned count; + + /* get registers for response, send an empty reply on error */ + if (registers_get(message[0], message[1], ®isters, &count) < 0) + count = 0; + + /* constrain count to requested size or message limit */ + if (count > message[2]) + count = message[2]; + if (count > HX_STREAM_MAX_FRAME) + count = HX_STREAM_MAX_FRAME; + + /* start sending the reply */ + sending = true; + hx_stream_reset(if_stream); + hx_stream_start(if_stream, registers, count * 2 + 2); + + /* enable the TX-ready interrupt */ + rCR1 |= USART_CR1_TXEIE; return; - uint16_t *registers; - unsigned count; - - tx_buf[0] = message[0]; - tx_buf[1] = message[1]; - - /* get registers for response, send an empty reply on error */ - if (registers_get(message[0], message[1], ®isters, &count) < 0) - count = 0; - - /* fill buffer with message, limited by length */ -#define TX_MAX ((sizeof(tx_buf) - 2) / 2) - if (count > TX_MAX) - count = TX_MAX; - if (count > message[2]) - count = message[2]; - memcpy(&tx_buf[2], registers, count * 2); - - /* try to send the message */ - hx_stream_send(if_stream, tx_buf, count * 2 + 2); + + } else { + + /* it's a write operation, pass it to the register API */ + registers_set(message[0], + message[1], + (const uint16_t *)&message[2], + (length - 2) / 2); + } } \ No newline at end of file diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index fdc3edac7..8e9c2bfcf 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -96,20 +96,6 @@ hx_tx_raw(hx_stream_t stream, uint8_t c) stream->tx_error = true; } -static void -hx_tx_byte(hx_stream_t stream, uint8_t c) -{ - switch (c) { - case FBO: - case CEO: - hx_tx_raw(stream, CEO); - c ^= 0x20; - break; - } - - hx_tx_raw(stream, c); -} - static int hx_rx_frame(hx_stream_t stream) { @@ -281,12 +267,12 @@ hx_stream_send(hx_stream_t stream, { int result; - result = hx_start(stream, data, count); + result = hx_stream_start(stream, data, count); if (result != OK) return result; int c; - while ((c = hx_send_next(stream)) >= 0) + while ((c = hx_stream_send_next(stream)) >= 0) hx_tx_raw(stream, c); /* check for transmit error */ -- cgit v1.2.3 From 843fb2d37179b82601a51e2d210052318f3301ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Jul 2013 09:29:06 +0400 Subject: position_estimator_inav init bugs fixed --- .../position_estimator_inav_main.c | 95 ++++++++++++++++------ 1 file changed, 68 insertions(+), 27 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 428269e4a..fb5a779bc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -62,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -162,7 +163,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_init_cnt = 0; - int baro_init_num = 70; /* measurement for 1 second */ + int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ double lat_current = 0.0; //[°]] --> 47.0 @@ -220,12 +221,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ bool wait_gps = params.use_gps; bool wait_baro = true; + hrt_abstime wait_gps_start = 0; + const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix - while (wait_gps || wait_baro) { - if (poll(fds_init, 2, 1000)) { + thread_running = true; + + while ((wait_gps || wait_baro) && !thread_should_exit) { + int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + + if (ret < 0) { + /* poll error */ + errx(1, "subscriptions poll error on init."); + + } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (wait_baro && sensor.baro_counter > baro_counter) { + baro_counter = sensor.baro_counter; + /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { baro_alt0 += sensor.baro_alt_meter; @@ -241,24 +255,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } - if (fds_init[1].revents & POLLIN) { + + if (params.use_gps && (fds_init[1].revents & POLLIN)) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (wait_gps && gps.fix_type >= 3) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; - - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = hrt_absolute_time(); - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + hrt_abstime t = hrt_absolute_time(); + + if (wait_gps_start == 0) { + wait_gps_start = t; + + } else if (t - wait_gps_start > wait_gps_delay) { + wait_gps = false; + /* get GPS position for first initialization */ + lat_current = gps.lat * 1e-7; + lon_current = gps.lon * 1e-7; + alt_current = gps.alt * 1e-3; + + local_pos.home_lat = lat_current * 1e7; + local_pos.home_lon = lon_current * 1e7; + local_pos.home_hdg = 0.0f; + local_pos.home_timestamp = t; + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + } } } } @@ -282,8 +305,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D float baro_corr = 0.0f; // D float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) }; float sonar_corr = 0.0f; float sonar_corr_filtered = 0.0f; @@ -293,7 +316,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime sonar_time = 0; /* main loop */ - struct pollfd fds[5] = { + struct pollfd fds[6] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -302,8 +325,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - thread_running = true; - warnx("main loop started."); + if (!thread_should_exit) { + warnx("main loop started."); + } while (!thread_should_exit) { int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate @@ -346,19 +370,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* transform acceleration vector from body frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; accel_corr[0] = accel_NED[0] - x_est[2]; accel_corr[1] = accel_NED[1] - y_est[2]; accel_corr[2] = accel_NED[2] - z_est[2]; + } else { memset(accel_corr, 0, sizeof(accel_corr)); } + accel_counter = sensor.accelerometer_counter; accel_updates++; } @@ -373,17 +402,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* optical flow */ if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { if (flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; sonar_corr = -flow.ground_distance_m - z_est[0]; sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (fabsf(sonar_corr) > params.sonar_err) { // correction is too large: spike or new ground level? if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { // spike detected, ignore sonar_corr = 0.0f; + } else { // new ground level baro_alt0 += sonar_corr; @@ -397,29 +429,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } + } else { sonar_corr = 0.0f; } + flow_updates++; } - if (params.use_gps && fds[5].revents & POLLIN) { + if (params.use_gps && (fds[5].revents & POLLIN)) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); gps_corr[0][0] = gps_proj[0] - x_est[0]; gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { gps_corr[0][1] = gps.vel_n_m_s; gps_corr[1][1] = gps.vel_e_m_s; + } else { gps_corr[0][1] = 0.0f; gps_corr[1][1] = 0.0f; } + gps_updates++; + } else { memset(gps_corr, 0, sizeof(gps_corr)); } @@ -442,6 +481,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* dont't try to estimate position when no any position source available */ bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + if (can_estimate_pos) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -454,6 +494,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (params.use_gps && gps.fix_type >= 3) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -505,8 +546,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.valid = local_pos.valid; double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t) (est_lat * 1e7); - global_pos.lon = (int32_t) (est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.alt = -local_pos.z - local_pos.home_alt; global_pos.relative_alt = -local_pos.z; global_pos.vx = local_pos.vx; -- cgit v1.2.3 From be6ad7af3b65841d2b460e3064c166dc9167401f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 00:08:12 -0700 Subject: Rework the FMU<->IO connection to use a simple fixed-size DMA packet; this should let us reduce overall latency and bump the bitrate up. Will still require some tuning. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 11 + src/drivers/boards/px4iov2/px4iov2_internal.h | 9 +- src/drivers/px4io/interface_serial.cpp | 343 ++++++++++++++------------ src/modules/px4iofirmware/serial.c | 169 ++++++++----- 4 files changed, 309 insertions(+), 223 deletions(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 78f6a2e65..1698336b4 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -58,6 +58,17 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + + /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index 282ed7548..b8aa6d053 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -59,8 +59,13 @@ /****************************************************************************** * Serial ******************************************************************************/ -#define SERIAL_BASE STM32_USART1_BASE -#define SERIAL_VECTOR STM32_IRQ_USART1 +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 1500000 /****************************************************************************** * GPIOS diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index d0af2912a..9471cecdd 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -59,14 +59,28 @@ #include -#include +#include +#include /* XXX should really not be hardcoding v2 here */ #include "interface.h" +const unsigned max_rw_regs = 32; // by agreement w/IO + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count; +#define PKT_CTRL_WRITE (1<<7) + uint8_t spare; + uint8_t page; + uint8_t offset; + uint16_t regs[max_rw_regs]; +}; +#pragma pack(pop) + class PX4IO_serial : public PX4IO_interface { public: - PX4IO_serial(int port); + PX4IO_serial(); virtual ~PX4IO_serial(); virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -75,119 +89,133 @@ public: virtual bool ok(); private: - volatile uint32_t *_serial_base; - int _vector; + /* + * XXX tune this value + * + * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. + * Packet overhead is 26µs for the four-byte header. + * + * 32 registers = 451µs + * + * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) + * transfers? Could cause issues with any regs expecting to be written atomically... + */ + static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory + + static const unsigned _serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP; + DMA_HANDLE _tx_dma; - uint8_t *_tx_buf; - unsigned _tx_size; + static const unsigned _serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP; + DMA_HANDLE _rx_dma; - const uint8_t *_rx_buf; - unsigned _rx_size; + /** set if we have started a transaction that expects a reply */ + bool _expect_reply; - hx_stream_t _stream; + /** saved DMA status (in case we care) */ + uint8_t _dma_status; + /** bus-ownership lock */ sem_t _bus_semaphore; - sem_t _completion_semaphore; - /** - * Send _tx_size bytes from the buffer, then - * if _rx_size is greater than zero wait for a packet - * to come back. - */ - int _wait_complete(); + /** client-waiting lock/signal */ + sem_t _completion_semaphore; /** - * Interrupt handler. + * Start the transaction with IO and wait for it to complete. + * + * @param expect_reply If true, expect a reply from IO. */ - static int _interrupt(int irq, void *context); - void _do_interrupt(); + int _wait_complete(bool expect_reply); /** - * Stream transmit callback + * DMA completion handler. */ - static void _tx(void *arg, uint8_t data); - void _do_tx(uint8_t data); + static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); + void _do_dma_callback(DMA_HANDLE handle, uint8_t status); /** - * Stream receive callback + * (re)configure the DMA */ - static void _rx(void *arg, const void *data, size_t length); - void _do_rx(const uint8_t *data, size_t length); + void _reset_dma(); /** * Serial register accessors. */ volatile uint32_t &_sreg(unsigned offset) { - return *(_serial_base + (offset / sizeof(uint32_t))); + return *(volatile uint32_t *)(PX4IO_SERIAL_BASE + offset); } - volatile uint32_t &_SR() { return _sreg(STM32_USART_SR_OFFSET); } - volatile uint32_t &_DR() { return _sreg(STM32_USART_DR_OFFSET); } - volatile uint32_t &_BRR() { return _sreg(STM32_USART_BRR_OFFSET); } - volatile uint32_t &_CR1() { return _sreg(STM32_USART_CR1_OFFSET); } - volatile uint32_t &_CR2() { return _sreg(STM32_USART_CR2_OFFSET); } - volatile uint32_t &_CR3() { return _sreg(STM32_USART_CR3_OFFSET); } - volatile uint32_t &_GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } + uint32_t _SR() { return _sreg(STM32_USART_SR_OFFSET); } + void _SR(uint32_t val) { _sreg(STM32_USART_SR_OFFSET) = val; } + uint32_t _DR() { return _sreg(STM32_USART_DR_OFFSET); } + void _DR(uint32_t val) { _sreg(STM32_USART_DR_OFFSET) = val; } + uint32_t _BRR() { return _sreg(STM32_USART_BRR_OFFSET); } + void _BRR(uint32_t val) { _sreg(STM32_USART_BRR_OFFSET) = val; } + uint32_t _CR1() { return _sreg(STM32_USART_CR1_OFFSET); } + void _CR1(uint32_t val) { _sreg(STM32_USART_CR1_OFFSET) = val; } + uint32_t _CR2() { return _sreg(STM32_USART_CR2_OFFSET); } + void _CR2(uint32_t val) { _sreg(STM32_USART_CR2_OFFSET) = val; } + uint32_t _CR3() { return _sreg(STM32_USART_CR3_OFFSET); } + void _CR3(uint32_t val) { _sreg(STM32_USART_CR3_OFFSET) = val; } + uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } + void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } + }; -/* XXX hack to avoid expensive IRQ lookup */ -static PX4IO_serial *io_serial; +IOPacket PX4IO_serial::_dma_buffer; PX4IO_interface *io_serial_interface(int port) { - return new PX4IO_serial(port); + return new PX4IO_serial(); } -PX4IO_serial::PX4IO_serial(int port) : - _serial_base(0), - _vector(0), - _tx_buf(nullptr), - _tx_size(0), - _rx_size(0), - _stream(0) +PX4IO_serial::PX4IO_serial() : + _tx_dma(nullptr), + _rx_dma(nullptr), + _expect_reply(false) { - /* only allow one instance */ - if (io_serial != nullptr) + /* allocate DMA */ + _tx_dma = stm32_dmachannel(_serial_tx_dma); + _rx_dma = stm32_dmachannel(_serial_rx_dma); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return; - switch (port) { - case 5: - _serial_base = (volatile uint32_t *)STM32_UART5_BASE; - _vector = STM32_IRQ_UART5; - break; - default: - /* not a supported port */ - return; - } + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); - /* XXX need to configure the port here */ + /* reset & configure the UART */ + _CR1(0); + _CR2(0); + _CR3(0); - /* need space for worst-case escapes + hx protocol overhead */ - /* XXX this is kinda gross, but hx transmits a byte at a time */ - _tx_buf = new uint8_t[HX_STREAM_MAX_FRAME]; + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - irq_attach(_vector, &_interrupt); + /* enable UART and DMA but no interrupts */ + _CR3(USART_CR3_DMAR | USART_CR3_DMAT); + _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE); - _stream = hx_stream_init(-1, _rx, this); + /* configure DMA */ + _reset_dma(); + /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); } PX4IO_serial::~PX4IO_serial() { + if (_tx_dma != nullptr) + stm32_dmafree(_tx_dma); + if (_rx_dma != nullptr) + stm32_dmafree(_rx_dma); - if (_tx_buf != nullptr) - delete[] _tx_buf; - - if (_vector) - irq_detach(_vector); - - if (io_serial == this) - io_serial = nullptr; - - if (_stream) - hx_stream_free(_stream); + stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); @@ -196,13 +224,9 @@ PX4IO_serial::~PX4IO_serial() bool PX4IO_serial::ok() { - if (_serial_base == 0) - return false; - if (_vector == 0) - return false; - if (_tx_buf == nullptr) + if (_tx_dma == nullptr) return false; - if (!_stream) + if (_rx_dma == nullptr) return false; return true; @@ -211,22 +235,20 @@ PX4IO_serial::ok() int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - - unsigned count = num_values * sizeof(*values); - if (count > (HX_STREAM_MAX_FRAME - 2)) + if (num_values > max_rw_regs) return -EINVAL; sem_wait(&_bus_semaphore); - _tx_buf[0] = page; - _tx_buf[1] = offset; - memcpy(&_tx_buf[2], (void *)values, count); - - _tx_size = count + 2; - _rx_size = 0; + _dma_buffer.count = num_values | PKT_CTRL_WRITE; + _dma_buffer.spare = 0; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + int result = _wait_complete(false); sem_post(&_bus_semaphore); return result; @@ -235,31 +257,28 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - - unsigned count = num_values * sizeof(*values); - if (count > HX_STREAM_MAX_FRAME) + if (num_values > max_rw_regs) return -EINVAL; sem_wait(&_bus_semaphore); - _tx_buf[0] = page; - _tx_buf[1] = offset; - _tx_buf[2] = num_values; - - _tx_size = 3; /* this tells IO that this is a read request */ - _rx_size = count; + _dma_buffer.count = num_values; + _dma_buffer.spare = 0; + _dma_buffer.page = page; + _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + int result = _wait_complete(true); if (result != OK) goto out; /* compare the received count with the expected count */ - if (_rx_size != count) { + if (_dma_buffer.count != num_values) { return -EIO; } else { + /* XXX implement check byte */ /* copy back the result */ - memcpy(values, &_rx_buf[0], count); + memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); } out: sem_post(&_bus_semaphore); @@ -267,14 +286,18 @@ out: } int -PX4IO_serial::_wait_complete() +PX4IO_serial::_wait_complete(bool expect_reply) { - /* prepare the stream for transmission (also discards any received noise) */ - hx_stream_reset(_stream); - hx_stream_start(_stream, _tx_buf, _tx_size); - /* enable transmit-ready interrupt, which will start transmission */ - _CR1() |= USART_CR1_TXEIE; + /* save for callback use */ + _expect_reply = expect_reply; + + /* start RX DMA */ + if (expect_reply) + stm32_dmastart(_rx_dma, _dma_callback, this, false); + + /* start TX DMA - no callback if we also expect a reply */ + stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false); /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -285,68 +308,82 @@ PX4IO_serial::_wait_complete() abstime.tv_nsec -= 1000000000; } - /* wait for the transaction to complete */ - int ret = sem_timedwait(&_completion_semaphore, &abstime); + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); - return ret; -} + if (ret == OK) + break; -int -PX4IO_serial::_interrupt(int irq, void *context) -{ - /* ... because NuttX doesn't give us a handle per vector */ - io_serial->_do_interrupt(); - return 0; -} - -void -PX4IO_serial::_do_interrupt() -{ - uint32_t sr = _SR(); - - /* handle transmit completion */ - if (sr & USART_SR_TXE) { - int c = hx_stream_send_next(_stream); - if (c == -1) { - /* no more bytes to send, not interested in interrupts now */ - _CR1() &= ~USART_CR1_TXEIE; - - /* was this a tx-only operation? */ - if (_rx_size == 0) { - /* wake up waiting sender */ - sem_post(&_completion_semaphore); - } - } else { - _DR() = c; + if (ret == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _reset_dma(); + break; } } - if (sr & USART_SR_RXNE) { - uint8_t c = _DR(); - - hx_stream_rx(_stream, c); - } + return ret; } void -PX4IO_serial::_rx(void *arg, const void *data, size_t length) +PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - PX4IO_serial *pserial = reinterpret_cast(arg); + if (arg != nullptr) { + PX4IO_serial *ps = reinterpret_cast(arg); - pserial->_do_rx((const uint8_t *)data, length); + ps->_do_dma_callback(handle, status); + } } void -PX4IO_serial::_do_rx(const uint8_t *data, size_t length) +PX4IO_serial::_do_dma_callback(DMA_HANDLE handle, uint8_t status) { - _rx_buf = data; - - if (length < _rx_size) - _rx_size = length; + /* on completion of a no-reply transmit, wake the sender */ + if (handle == _tx_dma) { + if ((status & DMA_STATUS_TCIF) && !_expect_reply) { + _dma_status = status; + sem_post(&_completion_semaphore); + } + /* XXX if we think we're going to see DMA errors, we should handle them here */ + } - /* notify waiting receiver */ - sem_post(&_completion_semaphore); + /* on completion of a reply, wake the waiter */ + if (handle == _rx_dma) { + if (status & DMA_STATUS_TCIF) { + /* XXX if we are worried about overrun/synch, check UART status here */ + _dma_status = status; + sem_post(&_completion_semaphore); + } + } } - - +void +PX4IO_serial::_reset_dma() +{ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); +} \ No newline at end of file diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index f691969c4..3fedfeb2c 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,14 +56,29 @@ //#define DEBUG #include "px4io.h" -static hx_stream_t if_stream; static volatile bool sending = false; -static int serial_interrupt(int vector, void *context); -static void serial_callback(void *arg, const void *data, unsigned length); +static void dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +#define MAX_RW_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count; +#define PKT_CTRL_WRITE (1<<7) + uint8_t spare; + uint8_t page; + uint8_t offset; + uint16_t regs[MAX_RW_REGS]; +}; +#pragma pack(pop) + +static struct IOPacket dma_packet; /* serial register accessors */ -#define REG(_x) (*(volatile uint32_t *)(SERIAL_BASE + _x)) +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) #define rSR REG(STM32_USART_SR_OFFSET) #define rDR REG(STM32_USART_DR_OFFSET) #define rBRR REG(STM32_USART_BRR_OFFSET) @@ -75,13 +90,51 @@ static void serial_callback(void *arg, const void *data, unsigned length); void interface_init(void) { - - /* XXX do serial port init here */ - - irq_attach(SERIAL_VECTOR, serial_interrupt); - if_stream = hx_stream_init(-1, serial_callback, NULL); - - /* XXX add stream stats counters? */ + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* enable UART and DMA */ + rCR3 = USART_CR3_DMAR | USART_CR3_DMAT; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE; + + /* configure DMA */ + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the first packet */ + stm32_dmastart(rx_dma, dma_callback, NULL, false); debug("serial init"); } @@ -89,76 +142,56 @@ interface_init(void) void interface_tick() { - /* XXX nothing interesting to do here */ + /* XXX look for stuck/damaged DMA and reset? */ } -static int -serial_interrupt(int vector, void *context) +static void +dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - uint32_t sr = rSR; - - if (sr & USART_SR_TXE) { - int c = hx_stream_send_next(if_stream); - if (c == -1) { - /* no more bytes to send, not interested in interrupts now */ - rCR1 &= ~USART_CR1_TXEIE; - sending = false; - } else { - rDR = c; - } + if (!(status & DMA_STATUS_TCIF)) { + /* XXX what do we do here? it's fatal! */ + return; } - if (sr & USART_SR_RXNE) { - uint8_t c = rDR; - - hx_stream_rx(if_stream, c); + /* if this is transmit-complete, make a note */ + if (handle == tx_dma) { + sending = false; + return; } - return 0; -} -static void -serial_callback(void *arg, const void *data, unsigned length) -{ - const uint8_t *message = (const uint8_t *)data; + /* we just received a request; sort out what to do */ + /* XXX implement check byte */ + /* XXX if we care about overruns, check the UART received-data-ready bit */ + bool send_reply = false; + if (dma_packet.count & PKT_CTRL_WRITE) { - /* malformed frame, ignore it */ - if (length < 2) - return; - - /* we got a new request while we were still sending the last reply - not supported */ - if (sending) - return; + /* it's a blind write - pass it on */ + registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count); + } else { - /* reads are page / offset / length */ - if (length == 3) { - uint16_t *registers; + /* it's a read - get register pointer for reply */ + int result; unsigned count; + uint16_t *registers; - /* get registers for response, send an empty reply on error */ - if (registers_get(message[0], message[1], ®isters, &count) < 0) + result = registers_get(dma_packet.page, dma_packet.offset, ®isters, &count); + if (result < 0) count = 0; - /* constrain count to requested size or message limit */ - if (count > message[2]) - count = message[2]; - if (count > HX_STREAM_MAX_FRAME) - count = HX_STREAM_MAX_FRAME; + /* constrain reply to packet size */ + if (count > MAX_RW_REGS) + count = MAX_RW_REGS; - /* start sending the reply */ - sending = true; - hx_stream_reset(if_stream); - hx_stream_start(if_stream, registers, count * 2 + 2); - - /* enable the TX-ready interrupt */ - rCR1 |= USART_CR1_TXEIE; - return; + /* copy reply registers into DMA buffer */ + send_reply = true; + memcpy((void *)&dma_packet.regs[0], registers, count); + dma_packet.count = count; + } - } else { + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + stm32_dmastart(rx_dma, dma_callback, NULL, false); - /* it's a write operation, pass it to the register API */ - registers_set(message[0], - message[1], - (const uint16_t *)&message[2], - (length - 2) / 2); - } -} \ No newline at end of file + /* if we have a reply to send, start that now */ + if (send_reply) + stm32_dmastart(tx_dma, dma_callback, NULL, false); +} -- cgit v1.2.3 From ea1f61e09388c0fd348dd54a46332bad939ffe00 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 22:22:10 -0700 Subject: USB console isn't working. Go back to UART8 which is. --- nuttx/configs/px4fmuv2/nsh/defconfig | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 8e69f321a..16be7cd41 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -234,10 +234,10 @@ STM32_FLASH_PREFETCH=y # CONFIG_USARTn_2STOP - Two stop bits # CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_CONSOLE_REINIT=y +CONFIG_SERIAL_CONSOLE_REINIT=n CONFIG_STANDARD_SERIAL=y -CONFIG_UART8_SERIAL_CONSOLE=n +CONFIG_UART8_SERIAL_CONSOLE=y #Mavlink messages can be bigger than 128 CONFIG_USART1_TXBUFSIZE=512 @@ -555,8 +555,8 @@ CONFIG_START_MONTH=1 CONFIG_START_DAY=1 CONFIG_GREGORIAN_TIME=n CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=n -CONFIG_DEV_LOWCONSOLE=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=y CONFIG_MUTEX_TYPES=n CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=0 @@ -943,7 +943,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512 # Size of the serial receive/transmit buffers. Default 256. # CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n #CONFIG_CDCACM_EP0MAXPACKET CONFIG_CDCACM_EPINTIN=1 #CONFIG_CDCACM_EPINTIN_FSSIZE @@ -1014,7 +1014,7 @@ CONFIG_NSH_ROMFSETC=y CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_CONSOLE=y CONFIG_NSH_USBCONSOLE=n -CONFIG_NSH_USBCONDEV="/dev/ttyACM0" +#CONFIG_NSH_USBCONDEV="/dev/ttyACM0" CONFIG_NSH_TELNET=n CONFIG_NSH_ARCHINIT=y CONFIG_NSH_IOBUFFER_SIZE=512 -- cgit v1.2.3 From 03a15bfdc5d3903240f040e5de5baac12bb3080f Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 22:23:01 -0700 Subject: Fix argument parsing in the rgbled command. --- src/drivers/rgbled/rgbled.cpp | 54 ++++++++++++++++++------------------------- 1 file changed, 23 insertions(+), 31 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index dc1e469c0..dae41d934 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -248,7 +248,7 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) void RGBLED::led_trampoline(void *arg) { - RGBLED *rgbl = (RGBLED *)arg; + RGBLED *rgbl = reinterpret_cast(arg); rgbl->led(); } @@ -413,35 +413,34 @@ void rgbled_usage(); void rgbled_usage() { warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); warnx("options:"); - warnx("\t-b --bus i2cbus (3)"); - warnx("\t-a --ddr addr (9)"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + errx(0, " -a addr (0x%x)", ADDR); } int rgbled_main(int argc, char *argv[]) { - int i2cdevice = PX4_I2C_BUS_LED; int rgbledadr = ADDR; /* 7bit */ - int x; - - for (x = 1; x < argc; x++) { - if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { - if (argc > x + 1) { - i2cdevice = atoi(argv[x + 1]); - } - } - - if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--addr") == 0) { - if (argc > x + 1) { - rgbledadr = atoi(argv[x + 1]); - } + int ch; + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(optarg, NULL, 0); + break; + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + default: + rgbled_usage(); } - } + argc -= optind; + argv += optind; + const char *verb = argv[0]; - if (!strcmp(argv[1], "start")) { + if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) errx(1, "already started"); @@ -459,39 +458,32 @@ rgbled_main(int argc, char *argv[]) exit(0); } - + /* need the driver past this point */ if (g_rgbled == nullptr) { fprintf(stderr, "not started\n"); rgbled_usage(); exit(0); } - if (!strcmp(argv[1], "test")) { + if (!strcmp(verb, "test")) { g_rgbled->setMode(LED_MODE_TEST); exit(0); } - if (!strcmp(argv[1], "systemstate")) { + if (!strcmp(verb, "systemstate")) { g_rgbled->setMode(LED_MODE_SYSTEMSTATE); exit(0); } - if (!strcmp(argv[1], "info")) { + if (!strcmp(verb, "info")) { g_rgbled->info(); exit(0); } - if (!strcmp(argv[1], "off")) { + if (!strcmp(verb, "off")) { g_rgbled->setMode(LED_MODE_OFF); exit(0); } - - /* things that require access to the device */ - int fd = open(RGBLED_DEVICE_PATH, 0); - if (fd < 0) - err(1, "can't open RGBLED device"); - rgbled_usage(); - exit(0); } -- cgit v1.2.3 From 7255c02c20ac11289a059503c4562be7bc23179b Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 3 Jul 2013 23:41:40 -0700 Subject: Add a test hook for the PX4IO interface. Wire up some simple tests for the serial interface. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signal quality looks good at 1.5Mbps. Transmit timing is ~450µs per packet, ~20µs packet-to-packet delay spinning in a loop transmitting. --- src/drivers/px4io/interface.h | 9 +- src/drivers/px4io/interface_i2c.cpp | 8 ++ src/drivers/px4io/interface_serial.cpp | 69 +++++++-- src/drivers/px4io/px4io.cpp | 252 ++++++++++++++++++--------------- 4 files changed, 210 insertions(+), 128 deletions(-) diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h index 834cb9e07..cb7da1081 100644 --- a/src/drivers/px4io/interface.h +++ b/src/drivers/px4io/interface.h @@ -72,7 +72,14 @@ public: * @return Zero on success. */ virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0; + + /** + * Perform an interface test. + * + * @param mode Optional test mode. + */ + virtual int test(unsigned mode) = 0; }; extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address); -extern PX4IO_interface *io_serial_interface(int port); +extern PX4IO_interface *io_serial_interface(); diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp index 6895a7e23..45a707883 100644 --- a/src/drivers/px4io/interface_i2c.cpp +++ b/src/drivers/px4io/interface_i2c.cpp @@ -69,6 +69,8 @@ public: virtual bool ok(); + virtual int test(unsigned mode); + private: static const unsigned _retries = 2; @@ -107,6 +109,12 @@ PX4IO_I2C::ok() return true; } +int +PX4IO_I2C::test(unsigned mode) +{ + return 0; +} + int PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 9471cecdd..817bcba8e 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -87,6 +87,7 @@ public: virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); virtual bool ok(); + virtual int test(unsigned mode); private: /* @@ -102,10 +103,7 @@ private: */ static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory - static const unsigned _serial_tx_dma = PX4IO_SERIAL_RX_DMAMAP; DMA_HANDLE _tx_dma; - - static const unsigned _serial_rx_dma = PX4IO_SERIAL_TX_DMAMAP; DMA_HANDLE _rx_dma; /** set if we have started a transaction that expects a reply */ @@ -164,7 +162,7 @@ private: IOPacket PX4IO_serial::_dma_buffer; -PX4IO_interface *io_serial_interface(int port) +PX4IO_interface *io_serial_interface() { return new PX4IO_serial(); } @@ -175,8 +173,8 @@ PX4IO_serial::PX4IO_serial() : _expect_reply(false) { /* allocate DMA */ - _tx_dma = stm32_dmachannel(_serial_tx_dma); - _rx_dma = stm32_dmachannel(_serial_rx_dma); + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return; @@ -209,14 +207,25 @@ PX4IO_serial::PX4IO_serial() : PX4IO_serial::~PX4IO_serial() { - if (_tx_dma != nullptr) + if (_tx_dma != nullptr) { + stm32_dmastop(_tx_dma); stm32_dmafree(_tx_dma); - if (_rx_dma != nullptr) + } + if (_rx_dma != nullptr) { + stm32_dmastop(_rx_dma); stm32_dmafree(_rx_dma); + } + /* reset the UART */ + _CR1(0); + _CR2(0); + _CR3(0); + + /* restore the GPIOs */ stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + /* and kill our semaphores */ sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); } @@ -232,6 +241,40 @@ PX4IO_serial::ok() return true; } +int +PX4IO_serial::test(unsigned mode) +{ + + switch (mode) { + case 0: + lowsyslog("test 0\n"); + + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + _CR3(_CR3() & ~(USART_CR3_DMAR | USART_CR3_DMAT)); + + for (;;) { + while (!(_SR() & USART_SR_TXE)) + ; + _DR(0x55); + } + return 0; + + case 1: + lowsyslog("test 1\n"); + { + uint16_t value = 0x5555; + for (;;) { + if (set_reg(0x55, 0x55, &value, 1) != OK) + return -2; + } + return 0; + } + } + return -1; +} + int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -297,16 +340,20 @@ PX4IO_serial::_wait_complete(bool expect_reply) stm32_dmastart(_rx_dma, _dma_callback, this, false); /* start TX DMA - no callback if we also expect a reply */ - stm32_dmastart(_tx_dma, _dma_callback, expect_reply ? nullptr : this, false); + stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); /* compute the deadline for a 5ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); +#if 1 + abstime.tv_sec++; +#else abstime.tv_nsec += 5000000; /* 5ms timeout */ while (abstime.tv_nsec > 1000000000) { abstime.tv_sec++; abstime.tv_nsec -= 1000000000; } +#endif /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; @@ -316,11 +363,13 @@ PX4IO_serial::_wait_complete(bool expect_reply) if (ret == OK) break; - if (ret == ETIMEDOUT) { + if (errno == ETIMEDOUT) { + lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); /* something has broken - clear out any partial DMA state and reconfigure */ _reset_dma(); break; } + lowsyslog("unexpected ret %d/%d\n", ret, errno); } return ret; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ecf50c859..663609aed 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -104,8 +104,8 @@ public: /** * Set the update rate for actuator outputs from FMU to IO. * - * @param rate The rate in Hz actuator outpus are sent to IO. - * Min 10 Hz, max 400 Hz + * @param rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz */ int set_update_rate(int rate); @@ -120,14 +120,14 @@ public: /** * Push failsafe values to IO. * - * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) - * @param len Number of channels, could up to 8 + * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param len Number of channels, could up to 8 */ int set_failsafe_values(const uint16_t *vals, unsigned len); /** - * Print the current status of IO - */ + * Print the current status of IO + */ void print_status(); private: @@ -1579,7 +1579,7 @@ start(int argc, char *argv[]) PX4IO_interface *interface; #if defined(CONFIG_ARCH_BOARD_PX4FMUV2) - interface = io_serial_interface(5); /* XXX wrong port! */ + interface = io_serial_interface(); #elif defined(CONFIG_ARCH_BOARD_PX4FMU) interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); #else @@ -1710,6 +1710,35 @@ monitor(void) } } +void +if_test(unsigned mode) +{ + PX4IO_interface *interface; + +#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) + interface = io_serial_interface(); +#elif defined(CONFIG_ARCH_BOARD_PX4FMU) + interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +#else +# error Unknown board - cannot select interface. +#endif + + if (interface == nullptr) + errx(1, "cannot alloc interface"); + + if (!interface->ok()) { + delete interface; + errx(1, "interface init failed"); + } else { + + int result = interface->test(mode); + delete interface; + errx(0, "test returned %d", result); + } + + exit(0); +} + } int @@ -1722,28 +1751,85 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); - if (!strcmp(argv[1], "limit")) { + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { + printf("[px4io] loaded, detaching first\n"); + /* stop the driver */ + delete g_dev; + } - if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); - } else { - errx(1, "missing argument (50 - 200 Hz)"); - return 1; - } + PX4IO_Uploader *up; + const char *fn[3]; + + /* work out what we're uploading... */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + + } else { + fn[0] = "/fs/microsd/px4io.bin"; + fn[1] = "/etc/px4io.bin"; + fn[2] = nullptr; + } + + up = new PX4IO_Uploader; + int ret = up->upload(&fn[0]); + delete up; + + switch (ret) { + case OK: + break; + + case -ENOENT: + errx(1, "PX4IO firmware file not found"); + + case -EEXIST: + case -EIO: + errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + + case -EINVAL: + errx(1, "verify failed - retry the update"); + + case -ETIMEDOUT: + errx(1, "timed out waiting for bootloader - power-cycle and try again"); + + default: + errx(1, "unexpected error %d", ret); + } + + return ret; + } + + if (!strcmp(argv[1], "iftest")) { + if (g_dev != nullptr) + errx(1, "can't iftest when started"); + + if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); + } + + /* commands below here require a started driver */ + + if (g_dev == nullptr) + errx(1, "not started"); + + if (!strcmp(argv[1], "limit")) { + + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); + } else { + errx(1, "missing argument (50 - 200 Hz)"); + return 1; } exit(0); } if (!strcmp(argv[1], "current")) { - if (g_dev != nullptr) { - if ((argc > 3)) { - g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); - } else { - errx(1, "missing argument (apm_per_volt, amp_offset)"); - return 1; - } + if ((argc > 3)) { + g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { + errx(1, "missing argument (apm_per_volt, amp_offset)"); + return 1; } exit(0); } @@ -1754,70 +1840,54 @@ px4io_main(int argc, char *argv[]) errx(1, "failsafe command needs at least one channel value (ppm)"); } - if (g_dev != nullptr) { + /* set values for first 8 channels, fill unassigned channels with 1500. */ + uint16_t failsafe[8]; - /* set values for first 8 channels, fill unassigned channels with 1500. */ - uint16_t failsafe[8]; - - for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < 800 || failsafe[i] > 2200) { - errx(1, "value out of range of 800 < value < 2200. Aborting."); - } - } else { - /* a zero value will result in stopping to output any pulse */ - failsafe[i] = 0; + for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { + + /* set channel to commandline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + failsafe[i] = atoi(argv[i+2]); + if (failsafe[i] < 800 || failsafe[i] > 2200) { + errx(1, "value out of range of 800 < value < 2200. Aborting."); } + } else { + /* a zero value will result in stopping to output any pulse */ + failsafe[i] = 0; } + } - int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); - if (ret != OK) - errx(ret, "failed setting failsafe values"); - } else { - errx(1, "not loaded"); - } + if (ret != OK) + errx(ret, "failed setting failsafe values"); exit(0); } if (!strcmp(argv[1], "recovery")) { - if (g_dev != nullptr) { - /* - * Enable in-air restart support. - * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); - } else { - errx(1, "not loaded"); - } + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + exit(0); } if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } + /* stop the driver */ + delete g_dev; exit(0); } if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) { - printf("[px4io] loaded\n"); - g_dev->print_status(); - } else { - printf("[px4io] not loaded\n"); - } + printf("[px4io] loaded\n"); + g_dev->print_status(); exit(0); } @@ -1844,58 +1914,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - /* note, stop not currently implemented */ - - if (!strcmp(argv[1], "update")) { - - if (g_dev != nullptr) { - printf("[px4io] loaded, detaching first\n"); - /* stop the driver */ - delete g_dev; - } - - PX4IO_Uploader *up; - const char *fn[3]; - - /* work out what we're uploading... */ - if (argc > 2) { - fn[0] = argv[2]; - fn[1] = nullptr; - - } else { - fn[0] = "/fs/microsd/px4io.bin"; - fn[1] = "/etc/px4io.bin"; - fn[2] = nullptr; - } - - up = new PX4IO_Uploader; - int ret = up->upload(&fn[0]); - delete up; - - switch (ret) { - case OK: - break; - - case -ENOENT: - errx(1, "PX4IO firmware file not found"); - - case -EEXIST: - case -EIO: - errx(1, "error updating PX4IO - check that bootloader mode is enabled"); - - case -EINVAL: - errx(1, "verify failed - retry the update"); - - case -ETIMEDOUT: - errx(1, "timed out waiting for bootloader - power-cycle and try again"); - - default: - errx(1, "unexpected error %d", ret); - } - - return ret; - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -1909,6 +1927,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); - out: +out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); } -- cgit v1.2.3 From f2079ae7ff3459f5a72abeef419053fa8b7cfcf5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 11:21:25 -0700 Subject: Add DMA error handling. Add serial error handling. Add short-packet-reception handling (this may allow us to send/receive shorter packets… needs testing). MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/px4io/interface_serial.cpp | 285 ++++++++++++++++++++++++++------- 1 file changed, 225 insertions(+), 60 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 817bcba8e..2a05de174 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -52,6 +52,7 @@ #include /* XXX might be able to prune these */ +#include #include #include #include @@ -62,6 +63,8 @@ #include #include /* XXX should really not be hardcoding v2 here */ +#include + #include "interface.h" const unsigned max_rw_regs = 32; // by agreement w/IO @@ -77,6 +80,8 @@ struct IOPacket { }; #pragma pack(pop) +#define PKT_SIZE(_n) (4 + (2 * (_n))) + class PX4IO_serial : public PX4IO_interface { public: @@ -105,12 +110,13 @@ private: DMA_HANDLE _tx_dma; DMA_HANDLE _rx_dma; + volatile unsigned _rx_length; - /** set if we have started a transaction that expects a reply */ - bool _expect_reply; - - /** saved DMA status (in case we care) */ - uint8_t _dma_status; + /** saved DMA status */ + static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values + static const unsigned _dma_status_waiting = 0x00000000; + volatile unsigned _tx_dma_status; + volatile unsigned _rx_dma_status; /** bus-ownership lock */ sem_t _bus_semaphore; @@ -123,18 +129,25 @@ private: * * @param expect_reply If true, expect a reply from IO. */ - int _wait_complete(bool expect_reply); + int _wait_complete(bool expect_reply, unsigned tx_length); /** * DMA completion handler. */ static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); - void _do_dma_callback(DMA_HANDLE handle, uint8_t status); + void _do_tx_dma_callback(unsigned status); + void _do_rx_dma_callback(unsigned status); + + /** + * Serial interrupt handler. + */ + static int _interrupt(int vector, void *context); + void _do_interrupt(); /** - * (re)configure the DMA + * Cancel any DMA in progress with an error. */ - void _reset_dma(); + void _abort_dma(); /** * Serial register accessors. @@ -158,9 +171,19 @@ private: uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } + /** + * Performance counters. + */ + perf_counter_t _perf_dmasetup; + perf_counter_t _perf_timeouts; + perf_counter_t _perf_errors; + perf_counter_t _perf_wr; + perf_counter_t _perf_rd; + }; IOPacket PX4IO_serial::_dma_buffer; +static PX4IO_serial *g_interface; PX4IO_interface *io_serial_interface() { @@ -170,7 +193,14 @@ PX4IO_interface *io_serial_interface() PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), - _expect_reply(false) + _rx_length(0), + _tx_dma_status(_dma_status_inactive), + _rx_dma_status(_dma_status_inactive), + _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), + _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), + _perf_errors(perf_alloc(PC_COUNT, "errors ")), + _perf_wr(perf_alloc(PC_ELAPSED, "writes ")), + _perf_rd(perf_alloc(PC_ELAPSED, "reads ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -193,16 +223,19 @@ PX4IO_serial::PX4IO_serial() : uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - /* enable UART and DMA but no interrupts */ - _CR3(USART_CR3_DMAR | USART_CR3_DMAT); - _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE); + /* enable UART in DMA mode, enable error and line idle interrupts */ + _CR3(USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_EIE); + _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE); - /* configure DMA */ - _reset_dma(); + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); + + g_interface = this; } PX4IO_serial::~PX4IO_serial() @@ -221,6 +254,10 @@ PX4IO_serial::~PX4IO_serial() _CR2(0); _CR3(0); + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + /* restore the GPIOs */ stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); @@ -228,6 +265,9 @@ PX4IO_serial::~PX4IO_serial() /* and kill our semaphores */ sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); + + if (g_interface == this) + g_interface = nullptr; } bool @@ -265,9 +305,14 @@ PX4IO_serial::test(unsigned mode) lowsyslog("test 1\n"); { uint16_t value = 0x5555; - for (;;) { + for (unsigned count = 0;; count++) { if (set_reg(0x55, 0x55, &value, 1) != OK) return -2; + if (count > 10000) { + perf_print_counter(_perf_dmasetup); + perf_print_counter(_perf_wr); + count = 0; + } } return 0; } @@ -291,7 +336,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(false); + int result = _wait_complete(false, PKT_SIZE(num_values)); sem_post(&_bus_semaphore); return result; @@ -311,7 +356,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(true); + int result = _wait_complete(true, PKT_SIZE(0)); if (result != OK) goto out; @@ -329,24 +374,58 @@ out: } int -PX4IO_serial::_wait_complete(bool expect_reply) +PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) { - /* save for callback use */ - _expect_reply = expect_reply; - /* start RX DMA */ - if (expect_reply) + if (expect_reply) { + perf_begin(_perf_rd); + perf_begin(_perf_dmasetup); + + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + _rx_length = 0; + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); stm32_dmastart(_rx_dma, _dma_callback, this, false); + perf_end(_perf_dmasetup); + } else { + perf_begin(_perf_wr); + } + /* start TX DMA - no callback if we also expect a reply */ + /* DMA setup time ~3µs */ + perf_begin(_perf_dmasetup); + _tx_dma_status = _dma_status_waiting; + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), /* XXX should be tx_length */ + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); + perf_end(_perf_dmasetup); /* compute the deadline for a 5ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); #if 1 - abstime.tv_sec++; + abstime.tv_sec++; /* long timeout for testing */ #else abstime.tv_nsec += 5000000; /* 5ms timeout */ while (abstime.tv_nsec > 1000000000) { @@ -366,12 +445,32 @@ PX4IO_serial::_wait_complete(bool expect_reply) if (errno == ETIMEDOUT) { lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); /* something has broken - clear out any partial DMA state and reconfigure */ - _reset_dma(); + _abort_dma(); + perf_count(_perf_timeouts); break; } lowsyslog("unexpected ret %d/%d\n", ret, errno); } + /* check for DMA errors */ + if (ret == OK) { + if (_tx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA transmit error\n"); + ret = -1; + } + if (_rx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA receive error\n"); + ret = -1; + } + perf_count(_perf_errors); + } + + /* reset DMA status */ + _tx_dma_status = _dma_status_inactive; + _rx_dma_status = _dma_status_inactive; + + perf_end(expect_reply ? _perf_rd : _perf_wr); + return ret; } @@ -381,58 +480,124 @@ PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) if (arg != nullptr) { PX4IO_serial *ps = reinterpret_cast(arg); - ps->_do_dma_callback(handle, status); + if (handle == ps->_tx_dma) { + ps->_do_tx_dma_callback(status); + } else if (handle == ps->_rx_dma) { + ps->_do_rx_dma_callback(status); + } } } void -PX4IO_serial::_do_dma_callback(DMA_HANDLE handle, uint8_t status) +PX4IO_serial::_do_tx_dma_callback(unsigned status) { /* on completion of a no-reply transmit, wake the sender */ - if (handle == _tx_dma) { - if ((status & DMA_STATUS_TCIF) && !_expect_reply) { - _dma_status = status; + if (_tx_dma_status == _dma_status_waiting) { + + /* save TX status */ + _tx_dma_status = status; + + /* if we aren't going on to expect a reply, complete now */ + if (_rx_dma_status != _dma_status_waiting) sem_post(&_completion_semaphore); - } - /* XXX if we think we're going to see DMA errors, we should handle them here */ } +} + +void +PX4IO_serial::_do_rx_dma_callback(unsigned status) +{ + ASSERT(_tx_dma_status != _dma_status_waiting); /* on completion of a reply, wake the waiter */ - if (handle == _rx_dma) { - if (status & DMA_STATUS_TCIF) { - /* XXX if we are worried about overrun/synch, check UART status here */ - _dma_status = status; - sem_post(&_completion_semaphore); + if (_rx_dma_status == _dma_status_waiting) { + + /* check for packet overrun - this will occur after DMA completes */ + if (_SR() & (USART_SR_ORE | USART_SR_RXNE)) { + _DR(); + status = DMA_STATUS_TEIF; + } + + /* save RX status */ + _rx_dma_status = status; + + /* DMA may have stopped short */ + _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); + + /* complete now */ + sem_post(&_completion_semaphore); + } +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + if (g_interface != nullptr) + g_interface->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = _SR(); /* get UART status register */ + + /* handle error/exception conditions */ + if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { + /* read DR to clear status */ + _DR(); + } else { + ASSERT(0); + } + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* + * If we are in the process of listening for something, these are all fatal; + * abort the DMA with an error. + */ + if (_rx_dma_status == _dma_status_waiting) { + _abort_dma(); + return; + } + + /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ + + /* don't attempt to handle IDLE if it's set - things went bad */ + return; + } + + if (sr & USART_SR_IDLE) { + + /* if there was DMA transmission still going, this is an error */ + if (_tx_dma_status == _dma_status_waiting) { + + /* babble from IO */ + _abort_dma(); + return; + } + + /* if there is DMA reception going on, this is a short packet */ + if (_rx_dma_status == _dma_status_waiting) { + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TCIF); } } } void -PX4IO_serial::_reset_dma() +PX4IO_serial::_abort_dma() { stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); + _do_tx_dma_callback(DMA_STATUS_TEIF); + _do_rx_dma_callback(DMA_STATUS_TEIF); - stm32_dmasetup( - _tx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_DIR_M2P | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); - stm32_dmasetup( - _rx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_DIR_P2M | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); + _tx_dma_status = _dma_status_inactive; + _rx_dma_status = _dma_status_inactive; } \ No newline at end of file -- cgit v1.2.3 From 2e001aad0429c816321bfc76264282935911746e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 12:50:59 -0700 Subject: Add PX4IOv2 support to the uploader. --- src/drivers/boards/px4fmu/px4fmu_internal.h | 3 +++ src/drivers/boards/px4fmuv2/px4fmu_internal.h | 1 + src/drivers/px4io/uploader.cpp | 37 ++++++++++++++++++++++++--- 3 files changed, 37 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h index 671a58f8f..5a73c10bf 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu/px4fmu_internal.h @@ -58,6 +58,9 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS2" + //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" //#endif diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 1698336b4..dd291b9b7 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -59,6 +59,7 @@ __BEGIN_DECLS /* Configuration ************************************************************************************/ /* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS5" #define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX #define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX #define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 15524c3ae..67298af32 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -49,10 +49,21 @@ #include #include #include +#include #include #include "uploader.h" +#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#include +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU +#include +#endif + +// define for comms logging +//#define UDEBUG + static const uint32_t crctab[] = { 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, @@ -114,13 +125,23 @@ PX4IO_Uploader::upload(const char *filenames[]) const char *filename = NULL; size_t fw_size; - _io_fd = open("/dev/ttyS2", O_RDWR); +#ifndef PX4IO_SERIAL_DEVICE +#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload +#endif + + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); if (_io_fd < 0) { log("could not open interface"); return -errno; } + /* adjust line speed to match bootloader */ + struct termios t; + tcgetattr(_io_fd, &t); + cfsetspeed(&t, 115200); + tcsetattr(_io_fd, TCSANOW, &t); + /* look for the bootloader */ ret = sync(); @@ -251,12 +272,16 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { +#ifdef UDEBUG log("poll timeout %d", ret); +#endif return -ETIMEDOUT; } read(_io_fd, &c, 1); - //log("recv 0x%02x", c); +#ifdef UDEBUG + log("recv 0x%02x", c); +#endif return OK; } @@ -282,16 +307,20 @@ PX4IO_Uploader::drain() do { ret = recv(c, 1000); +#ifdef UDEBUG if (ret == OK) { - //log("discard 0x%02x", c); + log("discard 0x%02x", c); } +#endif } while (ret == OK); } int PX4IO_Uploader::send(uint8_t c) { - //log("send 0x%02x", c); +#ifdef UDEBUG + log("send 0x%02x", c); +#endif if (write(_io_fd, &c, 1) != 1) return -errno; -- cgit v1.2.3 From f7963a8c84792ff6d95fa52d92f308c596e309e4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:14:13 -0700 Subject: Fix printing of PC_COUNT perf counters --- Debug/PX4 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Debug/PX4 b/Debug/PX4 index 085cffe43..e99228ee0 100644 --- a/Debug/PX4 +++ b/Debug/PX4 @@ -27,7 +27,7 @@ define _perf_print # PC_COUNT if $hdr->type == 0 set $count = (struct perf_ctr_count *)$hdr - printf "%llu events,\n", $count->event_count; + printf "%llu events\n", $count->event_count end # PC_ELPASED if $hdr->type == 1 -- cgit v1.2.3 From c21237667bc2802f675c74641d25f538db5f2c0c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:16:13 -0700 Subject: iov2 pin definition cleanup sweep --- src/drivers/boards/px4iov2/px4iov2_init.c | 19 +++++-------------- src/drivers/boards/px4iov2/px4iov2_internal.h | 19 +++++++------------ src/modules/px4iofirmware/controls.c | 4 ++-- src/modules/px4iofirmware/px4io.c | 2 ++ src/modules/px4iofirmware/px4io.h | 4 +++- 5 files changed, 19 insertions(+), 29 deletions(-) diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c index 09469d7e8..5d7b22560 100644 --- a/src/drivers/boards/px4iov2/px4iov2_init.c +++ b/src/drivers/boards/px4iov2/px4iov2_init.c @@ -104,31 +104,25 @@ __EXPORT void stm32_boardinitialize(void) /* configure GPIOs */ - /* turn off - all leds are active low */ - stm32_gpiowrite(GPIO_LED1, true); - stm32_gpiowrite(GPIO_LED2, true); - stm32_gpiowrite(GPIO_LED3, true); + /* LEDS - default to off */ stm32_configgpio(GPIO_LED1); stm32_configgpio(GPIO_LED2); stm32_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_BTN_SAFETY); /* spektrum power enable is active high - disable it by default */ + /* XXX might not want to do this on warm restart? */ stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); - /* servo power enable is active low, and has a pull down resistor - * to keep it low during boot (since it may power the whole board.) - */ - stm32_gpiowrite(GPIO_SERVO_PWR_EN, false); - stm32_configgpio(GPIO_SERVO_PWR_EN); - stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + /* RSSI inputs */ stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ stm32_configgpio(GPIO_ADC_RSSI); + + /* servo rail voltage */ stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ @@ -140,7 +134,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); - stm32_configgpio(GPIO_PPM); /* xxx alternate function */ stm32_gpiowrite(GPIO_PWM1, false); @@ -166,6 +159,4 @@ __EXPORT void stm32_boardinitialize(void) stm32_gpiowrite(GPIO_PWM8, false); stm32_configgpio(GPIO_PWM8); - -// message("[boot] Successfully initialized px4iov2 gpios\n"); } diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index b8aa6d053..81a75431c 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -60,6 +60,7 @@ * Serial ******************************************************************************/ #define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 #define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX #define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX #define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX @@ -73,13 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) - -#define GPIO_LED_BLUE BOARD_GPIO_LED1 -#define GPIO_LED_AMBER BOARD_GPIO_LED2 -#define GPIO_LED_SAFETY BOARD_GPIO_LED3 +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ @@ -89,17 +86,14 @@ #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) - -#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) - +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) /* Analog inputs **************************************************************/ #define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) + /* the same rssi signal goes to both an adc and a timer input */ #define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) -/* floating pin */ #define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) /* PWM pins **************************************************************/ @@ -117,6 +111,7 @@ /* SBUS pins *************************************************************/ +/* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) #define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..95103964e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,10 +59,10 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 385920d53..e70b3fe88 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -143,7 +143,9 @@ user_start(int argc, char *argv[]) LED_SAFETY(false); /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif /* start the safety switch handler */ safety_init(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 47bcb8ddf..4965d0724 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -128,7 +128,9 @@ extern struct sys_state_s system_state; #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#ifdef GPIO_SERVO_PWR_EN +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#endif #ifdef GPIO_ACC1_PWR_EN # define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) #endif -- cgit v1.2.3 From 52096f017f170ac873b782bc4ac260851ad01d89 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:17:16 -0700 Subject: Switch to the 'normal' way of doing register accessors. Be more aggressive en/disabling DMA in the UART, since ST say you should. --- src/drivers/px4io/interface_serial.cpp | 139 +++++++++++++++++++-------------- 1 file changed, 82 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 2a05de174..5d9eac076 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -65,6 +65,8 @@ #include +#include + #include "interface.h" const unsigned max_rw_regs = 32; // by agreement w/IO @@ -80,6 +82,16 @@ struct IOPacket { }; #pragma pack(pop) +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + #define PKT_SIZE(_n) (4 + (2 * (_n))) class PX4IO_serial : public PX4IO_interface @@ -149,28 +161,6 @@ private: */ void _abort_dma(); - /** - * Serial register accessors. - */ - volatile uint32_t &_sreg(unsigned offset) - { - return *(volatile uint32_t *)(PX4IO_SERIAL_BASE + offset); - } - uint32_t _SR() { return _sreg(STM32_USART_SR_OFFSET); } - void _SR(uint32_t val) { _sreg(STM32_USART_SR_OFFSET) = val; } - uint32_t _DR() { return _sreg(STM32_USART_DR_OFFSET); } - void _DR(uint32_t val) { _sreg(STM32_USART_DR_OFFSET) = val; } - uint32_t _BRR() { return _sreg(STM32_USART_BRR_OFFSET); } - void _BRR(uint32_t val) { _sreg(STM32_USART_BRR_OFFSET) = val; } - uint32_t _CR1() { return _sreg(STM32_USART_CR1_OFFSET); } - void _CR1(uint32_t val) { _sreg(STM32_USART_CR1_OFFSET) = val; } - uint32_t _CR2() { return _sreg(STM32_USART_CR2_OFFSET); } - void _CR2(uint32_t val) { _sreg(STM32_USART_CR2_OFFSET) = val; } - uint32_t _CR3() { return _sreg(STM32_USART_CR3_OFFSET); } - void _CR3(uint32_t val) { _sreg(STM32_USART_CR3_OFFSET) = val; } - uint32_t _GTPR() { return _sreg(STM32_USART_GTPR_OFFSET); } - void _GTPR(uint32_t val) { _sreg(STM32_USART_GTPR_OFFSET) = val; } - /** * Performance counters. */ @@ -213,24 +203,28 @@ PX4IO_serial::PX4IO_serial() : stm32_configgpio(PX4IO_SERIAL_RX_GPIO); /* reset & configure the UART */ - _CR1(0); - _CR2(0); - _CR3(0); + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; /* configure line speed */ uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; - _BRR((mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT)); - - /* enable UART in DMA mode, enable error and line idle interrupts */ - _CR3(USART_CR3_DMAR | USART_CR3_DMAT | USART_CR3_EIE); - _CR1(USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE); + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); /* attach serial interrupt handler */ irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); up_enable_irq(PX4IO_SERIAL_VECTOR); + /* enable UART in DMA mode, enable error and line idle interrupts */ + /* rCR3 = USART_CR3_EIE;*/ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; + /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); sem_init(&_bus_semaphore, 0, 1); @@ -250,9 +244,9 @@ PX4IO_serial::~PX4IO_serial() } /* reset the UART */ - _CR1(0); - _CR2(0); - _CR3(0); + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; /* detach our interrupt handler */ up_disable_irq(PX4IO_SERIAL_VECTOR); @@ -292,30 +286,35 @@ PX4IO_serial::test(unsigned mode) /* kill DMA, this is a PIO test */ stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); - _CR3(_CR3() & ~(USART_CR3_DMAR | USART_CR3_DMAT)); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); for (;;) { - while (!(_SR() & USART_SR_TXE)) + while (!(rSR & USART_SR_TXE)) ; - _DR(0x55); + rDR = 0x55; } return 0; case 1: lowsyslog("test 1\n"); { - uint16_t value = 0x5555; for (unsigned count = 0;; count++) { - if (set_reg(0x55, 0x55, &value, 1) != OK) + uint16_t value = count & 0xffff; + + if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != OK) return -2; - if (count > 10000) { + if (count > 100) { perf_print_counter(_perf_dmasetup); perf_print_counter(_perf_wr); count = 0; } + usleep(100000); } return 0; } + case 2: + lowsyslog("test 2\n"); + return 0; } return -1; } @@ -333,6 +332,9 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + for (unsigned i = num_values; i < max_rw_regs; i++) + _dma_buffer.regs[i] = 0x55aa; + /* XXX implement check byte */ /* start the transaction and wait for it to complete */ @@ -377,6 +379,10 @@ int PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) { + /* clear any lingering error status */ + (void)rSR; + (void)rDR; + /* start RX DMA */ if (expect_reply) { perf_begin(_perf_rd); @@ -397,6 +403,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; perf_end(_perf_dmasetup); } else { @@ -419,6 +426,8 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); + rCR3 |= USART_CR3_DMAT; + perf_end(_perf_dmasetup); /* compute the deadline for a 5ms timeout */ @@ -439,8 +448,18 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) for (;;) { ret = sem_timedwait(&_completion_semaphore, &abstime); - if (ret == OK) + if (ret == OK) { + /* check for DMA errors */ + if (_tx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA transmit error\n"); + ret = -1; + } + if (_rx_dma_status & DMA_STATUS_TEIF) { + lowsyslog("DMA receive error\n"); + ret = -1; + } break; + } if (errno == ETIMEDOUT) { lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); @@ -452,24 +471,14 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) lowsyslog("unexpected ret %d/%d\n", ret, errno); } - /* check for DMA errors */ - if (ret == OK) { - if (_tx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA transmit error\n"); - ret = -1; - } - if (_rx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA receive error\n"); - ret = -1; - } - perf_count(_perf_errors); - } - /* reset DMA status */ _tx_dma_status = _dma_status_inactive; _rx_dma_status = _dma_status_inactive; + /* update counters */ perf_end(expect_reply ? _perf_rd : _perf_wr); + if (ret != OK) + perf_count(_perf_errors); return ret; } @@ -497,6 +506,9 @@ PX4IO_serial::_do_tx_dma_callback(unsigned status) /* save TX status */ _tx_dma_status = status; + /* disable UART DMA */ + rCR3 &= ~USART_CR3_DMAT; + /* if we aren't going on to expect a reply, complete now */ if (_rx_dma_status != _dma_status_waiting) sem_post(&_completion_semaphore); @@ -512,14 +524,18 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) if (_rx_dma_status == _dma_status_waiting) { /* check for packet overrun - this will occur after DMA completes */ - if (_SR() & (USART_SR_ORE | USART_SR_RXNE)) { - _DR(); + uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { + (void)rDR; status = DMA_STATUS_TEIF; } /* save RX status */ _rx_dma_status = status; + /* disable UART DMA */ + rCR3 &= ~USART_CR3_DMAR; + /* DMA may have stopped short */ _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); @@ -539,12 +555,12 @@ PX4IO_serial::_interrupt(int irq, void *context) void PX4IO_serial::_do_interrupt() { - uint32_t sr = _SR(); /* get UART status register */ + uint32_t sr = rSR; /* get UART status register */ /* handle error/exception conditions */ if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { /* read DR to clear status */ - _DR(); + (void)rDR; } else { ASSERT(0); } @@ -593,8 +609,17 @@ PX4IO_serial::_do_interrupt() void PX4IO_serial::_abort_dma() { + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* stop DMA */ stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); + + /* complete DMA as though in error */ _do_tx_dma_callback(DMA_STATUS_TEIF); _do_rx_dma_callback(DMA_STATUS_TEIF); -- cgit v1.2.3 From 43210413a98dfe32302cd99c86847729100133fa Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:17:55 -0700 Subject: More test work on the px4io side of the serial interface. --- src/modules/px4iofirmware/protocol.h | 4 + src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 14 ++- src/modules/px4iofirmware/serial.c | 193 ++++++++++++++++++++++++++-------- 4 files changed, 169 insertions(+), 44 deletions(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0e40bff69..b19146b06 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,10 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4965d0724..0779ffd8f 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -172,7 +172,7 @@ extern void interface_tick(void); /** * Register space */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); /** diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 42554456c..b977375f4 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -185,7 +185,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; -void +int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -261,11 +261,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) - break; + return -1; offset++; values++; } + break; } + return 0; } static int @@ -451,6 +453,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* case PX4IO_RC_PAGE_CONFIG */ } + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + break; + default: return -1; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 3fedfeb2c..b51ff87d3 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -51,17 +51,28 @@ #include #include #include -#include +#include //#define DEBUG #include "px4io.h" static volatile bool sending = false; -static void dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static perf_counter_t pc_rx; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_regerr; + +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static void tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static DMA_HANDLE tx_dma; static DMA_HANDLE rx_dma; +static int serial_interrupt(int irq, void *context); +static void dma_reset(void); + #define MAX_RW_REGS 32 // by agreement w/FMU #pragma pack(push, 1) @@ -90,6 +101,13 @@ static struct IOPacket dma_packet; void interface_init(void) { + pc_rx = perf_alloc(PC_COUNT, "rx count"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + /* allocate DMA */ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); @@ -103,38 +121,37 @@ interface_init(void) rCR2 = 0; rCR3 = 0; + /* clear status/errors */ + (void)rSR; + (void)rDR; + /* configure line speed */ uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); - /* enable UART and DMA */ - rCR3 = USART_CR3_DMAR | USART_CR3_DMAT; - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE; + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); + up_enable_irq(PX4FMU_SERIAL_VECTOR); - /* configure DMA */ - stm32_dmasetup( - tx_dma, - (uint32_t)&rDR, - (uint32_t)&dma_packet, - sizeof(dma_packet), - DMA_CCR_DIR | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); + /* enable UART and DMA */ + /*rCR3 = USART_CR3_EIE; */ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; - stm32_dmasetup( - rx_dma, - (uint32_t)&rDR, - (uint32_t)&dma_packet, - sizeof(dma_packet), - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); +#if 0 /* keep this for signal integrity testing */ + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xa0; + } +#endif - /* start receive DMA ready for the first packet */ - stm32_dmastart(rx_dma, dma_callback, NULL, false); + /* configure RX DMA and return to listening state */ + dma_reset(); debug("serial init"); } @@ -146,27 +163,36 @@ interface_tick() } static void -dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - if (!(status & DMA_STATUS_TCIF)) { - /* XXX what do we do here? it's fatal! */ - return; - } - - /* if this is transmit-complete, make a note */ - if (handle == tx_dma) { - sending = false; - return; - } + sending = false; + rCR3 &= ~USART_CR3_DMAT; +} +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ /* we just received a request; sort out what to do */ + + rCR3 &= ~USART_CR3_DMAR; + + /* work out how big the packet actually is */ + //unsigned rx_length = sizeof(IOPacket) - stm32_dmaresidual(rx_dma); + /* XXX implement check byte */ - /* XXX if we care about overruns, check the UART received-data-ready bit */ + + perf_count(pc_rx); + + /* default to not sending a reply */ bool send_reply = false; if (dma_packet.count & PKT_CTRL_WRITE) { + dma_packet.count &= ~PKT_CTRL_WRITE; + /* it's a blind write - pass it on */ - registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count); + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count)) + perf_count(pc_regerr); + } else { /* it's a read - get register pointer for reply */ @@ -189,9 +215,94 @@ dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) } /* re-set DMA for reception first, so we are ready to receive before we start sending */ - stm32_dmastart(rx_dma, dma_callback, NULL, false); + /* XXX latency here could mean loss of back-to-back writes; do we want to always send an ack? */ + /* XXX always sending an ack would simplify the FMU side (always wait for reply) too */ + dma_reset(); /* if we have a reply to send, start that now */ - if (send_reply) - stm32_dmastart(tx_dma, dma_callback, NULL, false); + if (send_reply) { + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), /* XXX cut back to actual transmit size */ + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + sending = true; + stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAT; + } +} + +static int +serial_interrupt(int irq, void *context) +{ + uint32_t sr = rSR; /* get UART status register */ + + /* handle error/exception conditions */ + if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { + /* read DR to clear status */ + (void)rDR; + } else { + ASSERT(0); + } + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + perf_count(pc_errors); + if (sr & USART_SR_ORE) + perf_count(pc_ore); + if (sr & USART_SR_NE) + perf_count(pc_ne); + if (sr & USART_SR_FE) + perf_count(pc_fe); + + /* reset DMA state machine back to listening-for-packet */ + dma_reset(); + + /* don't attempt to handle IDLE if it's set - things went bad */ + return 0; + } + + if (sr & USART_SR_IDLE) { + + /* XXX if there was DMA transmission still going, this is an error */ + + /* stop the receive DMA */ + stm32_dmastop(rx_dma); + + /* and treat this like DMA completion */ + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; } + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} \ No newline at end of file -- cgit v1.2.3 From 94b638d848a62312a9cd6722779965d211565d3c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:19:24 -0700 Subject: One more piece of paranoia when resetting DMA --- src/modules/px4iofirmware/serial.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index b51ff87d3..93cff26c2 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -286,7 +286,9 @@ static void dma_reset(void) { rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - + (void)rSR; + (void)rDR; + (void)rDR; /* kill any pending DMA */ stm32_dmastop(tx_dma); -- cgit v1.2.3 From 83213c66df27e41d5941ff21a4249df3f6f5f45e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:22:59 -0700 Subject: Reset the PX4IO rx DMA if we haven't seen any traffic in a while; this gets us back into sync. --- src/modules/px4iofirmware/serial.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 93cff26c2..4eef99d9f 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -73,6 +73,9 @@ static DMA_HANDLE rx_dma; static int serial_interrupt(int irq, void *context); static void dma_reset(void); +/* if we spend this many ticks idle, reset the DMA */ +static unsigned idle_ticks; + #define MAX_RW_REGS 32 // by agreement w/FMU #pragma pack(push, 1) @@ -160,6 +163,10 @@ void interface_tick() { /* XXX look for stuck/damaged DMA and reset? */ + if (idle_ticks++ > 100) { + dma_reset(); + idle_ticks = 0; + } } static void @@ -175,6 +182,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* we just received a request; sort out what to do */ rCR3 &= ~USART_CR3_DMAR; + idle_ticks = 0; /* work out how big the packet actually is */ //unsigned rx_length = sizeof(IOPacket) - stm32_dmaresidual(rx_dma); -- cgit v1.2.3 From d83323d4a28c0beb9686bd28193344b6b3079f00 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:18:55 -0700 Subject: Use the NuttX built-in crc32, it works fine. --- src/drivers/px4io/uploader.cpp | 50 ++++-------------------------------------- 1 file changed, 4 insertions(+), 46 deletions(-) diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 67298af32..28c584438 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -52,6 +52,8 @@ #include #include +#include + #include "uploader.h" #ifdef CONFIG_ARCH_BOARD_PX4FMUV2 @@ -64,50 +66,6 @@ // define for comms logging //#define UDEBUG -static const uint32_t crctab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -static uint32_t -crc32(const uint8_t *src, unsigned len, unsigned state) -{ - for (unsigned i = 0; i < len; i++) - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); - return state; -} - PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), _fw_fd(-1) @@ -594,14 +552,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) return -errno; /* calculate crc32 sum */ - sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum); + sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); bytes_read += count; } /* fill the rest with 0xff */ while (bytes_read < fw_size_remote) { - sum = crc32(&fill_blank, sizeof(fill_blank), sum); + sum = crc32part(&fill_blank, sizeof(fill_blank), sum); bytes_read += sizeof(fill_blank); } -- cgit v1.2.3 From e55a37697d56bfbec3bcd1febc9f0e185663f45d Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:34:44 -0700 Subject: Always send and expect a reply for every message. --- src/drivers/px4io/interface_serial.cpp | 60 +++++++++++++++------------------- src/modules/px4iofirmware/serial.c | 28 +++++++--------- 2 files changed, 39 insertions(+), 49 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 5d9eac076..1ce9e9c6d 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -167,8 +167,7 @@ private: perf_counter_t _perf_dmasetup; perf_counter_t _perf_timeouts; perf_counter_t _perf_errors; - perf_counter_t _perf_wr; - perf_counter_t _perf_rd; + perf_counter_t _perf_txns; }; @@ -189,8 +188,7 @@ PX4IO_serial::PX4IO_serial() : _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), _perf_errors(perf_alloc(PC_COUNT, "errors ")), - _perf_wr(perf_alloc(PC_ELAPSED, "writes ")), - _perf_rd(perf_alloc(PC_ELAPSED, "reads ")) + _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -305,10 +303,12 @@ PX4IO_serial::test(unsigned mode) return -2; if (count > 100) { perf_print_counter(_perf_dmasetup); - perf_print_counter(_perf_wr); + perf_print_counter(_perf_txns); + perf_print_counter(_perf_timeouts); + perf_print_counter(_perf_errors); count = 0; } - usleep(100000); + usleep(10000); } return 0; } @@ -384,35 +384,29 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) (void)rDR; /* start RX DMA */ - if (expect_reply) { - perf_begin(_perf_rd); - perf_begin(_perf_dmasetup); - - /* DMA setup time ~3µs */ - _rx_dma_status = _dma_status_waiting; - _rx_length = 0; - stm32_dmasetup( - _rx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_DIR_P2M | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); - stm32_dmastart(_rx_dma, _dma_callback, this, false); - rCR3 |= USART_CR3_DMAR; - - perf_end(_perf_dmasetup); - } else { - perf_begin(_perf_wr); - } + perf_begin(_perf_txns); + perf_begin(_perf_dmasetup); + + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + _rx_length = 0; + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; + /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ - perf_begin(_perf_dmasetup); _tx_dma_status = _dma_status_waiting; stm32_dmasetup( _tx_dma, @@ -476,7 +470,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) _rx_dma_status = _dma_status_inactive; /* update counters */ - perf_end(expect_reply ? _perf_rd : _perf_wr); + perf_end(_perf_txns); if (ret != OK) perf_count(_perf_errors); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 4eef99d9f..c109cb57f 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -192,7 +192,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) perf_count(pc_rx); /* default to not sending a reply */ - bool send_reply = false; if (dma_packet.count & PKT_CTRL_WRITE) { dma_packet.count &= ~PKT_CTRL_WRITE; @@ -217,7 +216,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) count = MAX_RW_REGS; /* copy reply registers into DMA buffer */ - send_reply = true; memcpy((void *)&dma_packet.regs[0], registers, count); dma_packet.count = count; } @@ -228,20 +226,18 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) dma_reset(); /* if we have a reply to send, start that now */ - if (send_reply) { - stm32_dmasetup( - tx_dma, - (uint32_t)&rDR, - (uint32_t)&dma_packet, - sizeof(dma_packet), /* XXX cut back to actual transmit size */ - DMA_CCR_DIR | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); - sending = true; - stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); - rCR3 |= USART_CR3_DMAT; - } + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), /* XXX cut back to actual transmit size */ + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + sending = true; + stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAT; } static int -- cgit v1.2.3 From 313231566c936927eef1fd4a8fc7012122342941 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:41:27 -0700 Subject: Encode the packet type and result in the unused high bits of the word count. --- src/drivers/px4io/interface_serial.cpp | 26 ++++++++++++++++++-------- src/modules/px4iofirmware/serial.c | 26 +++++++++++++++++++------- 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 1ce9e9c6d..ffd852cf0 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -73,8 +73,7 @@ const unsigned max_rw_regs = 32; // by agreement w/IO #pragma pack(push, 1) struct IOPacket { - uint8_t count; -#define PKT_CTRL_WRITE (1<<7) + uint8_t count_code; uint8_t spare; uint8_t page; uint8_t offset; @@ -92,7 +91,18 @@ struct IOPacket { #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -#define PKT_SIZE(_n) (4 + (2 * (_n))) +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) class PX4IO_serial : public PX4IO_interface { @@ -327,7 +337,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi sem_wait(&_bus_semaphore); - _dma_buffer.count = num_values | PKT_CTRL_WRITE; + _dma_buffer.count_code = num_values | PKT_CODE_WRITE; _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; @@ -338,7 +348,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(false, PKT_SIZE(num_values)); + int result = _wait_complete(false, PKT_SIZE(_dma_buffer)); sem_post(&_bus_semaphore); return result; @@ -352,18 +362,18 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n sem_wait(&_bus_semaphore); - _dma_buffer.count = num_values; + _dma_buffer.count_code = num_values | PKT_CODE_READ; _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(true, PKT_SIZE(0)); + int result = _wait_complete(true, PKT_SIZE(_dma_buffer)); if (result != OK) goto out; /* compare the received count with the expected count */ - if (_dma_buffer.count != num_values) { + if (PKT_COUNT(_dma_buffer) != num_values) { return -EIO; } else { /* XXX implement check byte */ diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index c109cb57f..64ca6195a 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -80,8 +80,7 @@ static unsigned idle_ticks; #pragma pack(push, 1) struct IOPacket { - uint8_t count; -#define PKT_CTRL_WRITE (1<<7) + uint8_t count_code; uint8_t spare; uint8_t page; uint8_t offset; @@ -89,6 +88,21 @@ struct IOPacket { }; #pragma pack(pop) +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +//static uint8_t crc_packet(void); + static struct IOPacket dma_packet; /* serial register accessors */ @@ -192,12 +206,10 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) perf_count(pc_rx); /* default to not sending a reply */ - if (dma_packet.count & PKT_CTRL_WRITE) { - - dma_packet.count &= ~PKT_CTRL_WRITE; + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ - if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], dma_packet.count)) + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) perf_count(pc_regerr); } else { @@ -217,7 +229,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count); - dma_packet.count = count; + dma_packet.count_code = count | PKT_CODE_SUCCESS; } /* re-set DMA for reception first, so we are ready to receive before we start sending */ -- cgit v1.2.3 From 5a8f874166e92ccb1d3a108164f403f622787a89 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 16:56:47 -0700 Subject: Add an 8-bit CRC to each transmitted packet. --- src/drivers/px4io/interface_serial.cpp | 60 +++++++++++++++++++++++++++++++--- src/modules/px4iofirmware/serial.c | 57 ++++++++++++++++++++++++++++++-- 2 files changed, 110 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index ffd852cf0..e688d672c 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -74,7 +74,7 @@ const unsigned max_rw_regs = 32; // by agreement w/IO #pragma pack(push, 1) struct IOPacket { uint8_t count_code; - uint8_t spare; + uint8_t crc; uint8_t page; uint8_t offset; uint16_t regs[max_rw_regs]; @@ -104,6 +104,9 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) +static uint8_t crc_packet(IOPacket &pkt); + + class PX4IO_serial : public PX4IO_interface { public: @@ -338,7 +341,6 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi sem_wait(&_bus_semaphore); _dma_buffer.count_code = num_values | PKT_CODE_WRITE; - _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); @@ -363,7 +365,6 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n sem_wait(&_bus_semaphore); _dma_buffer.count_code = num_values | PKT_CODE_READ; - _dma_buffer.spare = 0; _dma_buffer.page = page; _dma_buffer.offset = offset; @@ -417,6 +418,8 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ + _dma_buffer.crc = 0; + _dma_buffer.crc = crc_packet(_dma_buffer); _tx_dma_status = _dma_status_waiting; stm32_dmasetup( _tx_dma, @@ -629,4 +632,53 @@ PX4IO_serial::_abort_dma() _tx_dma_status = _dma_status_inactive; _rx_dma_status = _dma_status_inactive; -} \ No newline at end of file +} + +static const uint8_t crc8_tab[256] = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t +crc_packet(IOPacket &pkt) +{ + uint8_t *end = (uint8_t *)(&pkt.regs[PKT_COUNT(pkt)]); + uint8_t *p = (uint8_t *)&pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 64ca6195a..722d05809 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -81,7 +81,7 @@ static unsigned idle_ticks; #pragma pack(push, 1) struct IOPacket { uint8_t count_code; - uint8_t spare; + uint8_t crc; uint8_t page; uint8_t offset; uint16_t regs[MAX_RW_REGS]; @@ -101,7 +101,7 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) -//static uint8_t crc_packet(void); +static uint8_t crc_packet(void); static struct IOPacket dma_packet; @@ -238,6 +238,8 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) dma_reset(); /* if we have a reply to send, start that now */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(); stm32_dmasetup( tx_dma, (uint32_t)&rDR, @@ -323,4 +325,53 @@ dma_reset(void) /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); rCR3 |= USART_CR3_DMAR; -} \ No newline at end of file +} + +static const uint8_t crc8_tab[256] = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t +crc_packet() +{ + uint8_t *end = (uint8_t *)(&dma_packet.regs[PKT_COUNT(dma_packet)]); + uint8_t *p = (uint8_t *)&dma_packet; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} -- cgit v1.2.3 From 50cae347b41b66d236c26ea0dfdeb99b65824ebb Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 17:13:54 -0700 Subject: Check packet CRCs and count errors; don't reject packets yet. --- src/drivers/px4io/interface_serial.cpp | 13 ++++++++++++- src/modules/px4iofirmware/serial.c | 7 +++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index e688d672c..814f66ea4 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -181,6 +181,7 @@ private: perf_counter_t _perf_timeouts; perf_counter_t _perf_errors; perf_counter_t _perf_txns; + perf_counter_t _perf_crcerrs; }; @@ -201,7 +202,8 @@ PX4IO_serial::PX4IO_serial() : _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), _perf_errors(perf_alloc(PC_COUNT, "errors ")), - _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) + _perf_txns(perf_alloc(PC_ELAPSED, "txns ")), + _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -319,6 +321,7 @@ PX4IO_serial::test(unsigned mode) perf_print_counter(_perf_txns); perf_print_counter(_perf_timeouts); perf_print_counter(_perf_errors); + perf_print_counter(_perf_crcerrs); count = 0; } usleep(10000); @@ -460,11 +463,19 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) if (_tx_dma_status & DMA_STATUS_TEIF) { lowsyslog("DMA transmit error\n"); ret = -1; + break; } if (_rx_dma_status & DMA_STATUS_TEIF) { lowsyslog("DMA receive error\n"); ret = -1; + break; } + + /* check packet CRC */ + uint8_t crc = _dma_buffer.crc; + _dma_buffer.crc = 0; + if (crc != crc_packet(_dma_buffer)) + perf_count(_perf_crcerrs); break; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 722d05809..6c6a9a2b1 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -64,6 +64,7 @@ static perf_counter_t pc_ore; static perf_counter_t pc_fe; static perf_counter_t pc_ne; static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static void tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); @@ -124,6 +125,7 @@ interface_init(void) pc_fe = perf_alloc(PC_COUNT, "framing"); pc_ne = perf_alloc(PC_COUNT, "noise"); pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); /* allocate DMA */ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); @@ -205,6 +207,11 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) perf_count(pc_rx); + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + if (crc != crc_packet()) + perf_count(pc_crcerr); + /* default to not sending a reply */ if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { -- cgit v1.2.3 From 6c5a15da9b611d52a70af0ffd3d97bf757b974f5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 17:39:28 -0700 Subject: Eliminate the TD DMA callback; we don't need to know that it's completed. Fix abort behaviour on timeouts, now we don't wedge after the first one. --- src/drivers/px4io/interface_serial.cpp | 75 ++++++++-------------------------- 1 file changed, 18 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 814f66ea4..1af64379b 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -140,7 +140,6 @@ private: /** saved DMA status */ static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values static const unsigned _dma_status_waiting = 0x00000000; - volatile unsigned _tx_dma_status; volatile unsigned _rx_dma_status; /** bus-ownership lock */ @@ -151,16 +150,13 @@ private: /** * Start the transaction with IO and wait for it to complete. - * - * @param expect_reply If true, expect a reply from IO. */ - int _wait_complete(bool expect_reply, unsigned tx_length); + int _wait_complete(); /** * DMA completion handler. */ static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); - void _do_tx_dma_callback(unsigned status); void _do_rx_dma_callback(unsigned status); /** @@ -197,7 +193,6 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_length(0), - _tx_dma_status(_dma_status_inactive), _rx_dma_status(_dma_status_inactive), _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), @@ -314,8 +309,9 @@ PX4IO_serial::test(unsigned mode) for (unsigned count = 0;; count++) { uint16_t value = count & 0xffff; - if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != OK) - return -2; + set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1); + /* ignore errors */ + if (count > 100) { perf_print_counter(_perf_dmasetup); perf_print_counter(_perf_txns); @@ -353,7 +349,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi /* XXX implement check byte */ /* start the transaction and wait for it to complete */ - int result = _wait_complete(false, PKT_SIZE(_dma_buffer)); + int result = _wait_complete(); sem_post(&_bus_semaphore); return result; @@ -372,13 +368,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n _dma_buffer.offset = offset; /* start the transaction and wait for it to complete */ - int result = _wait_complete(true, PKT_SIZE(_dma_buffer)); + int result = _wait_complete(); if (result != OK) goto out; /* compare the received count with the expected count */ if (PKT_COUNT(_dma_buffer) != num_values) { - return -EIO; + result = -EIO; + goto out; } else { /* XXX implement check byte */ /* copy back the result */ @@ -386,13 +383,12 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n } out: sem_post(&_bus_semaphore); - return OK; + return result; } int -PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) +PX4IO_serial::_wait_complete() { - /* clear any lingering error status */ (void)rSR; (void)rDR; @@ -418,12 +414,10 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) stm32_dmastart(_rx_dma, _dma_callback, this, false); rCR3 |= USART_CR3_DMAR; - /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ _dma_buffer.crc = 0; _dma_buffer.crc = crc_packet(_dma_buffer); - _tx_dma_status = _dma_status_waiting; stm32_dmasetup( _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, @@ -435,7 +429,7 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) DMA_SCR_MSIZE_8BITS | DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); - stm32_dmastart(_tx_dma, /*expect_reply ? nullptr :*/ _dma_callback, this, false); + stm32_dmastart(_tx_dma, nullptr, nullptr, false); rCR3 |= USART_CR3_DMAT; perf_end(_perf_dmasetup); @@ -460,11 +454,6 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) if (ret == OK) { /* check for DMA errors */ - if (_tx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA transmit error\n"); - ret = -1; - break; - } if (_rx_dma_status & DMA_STATUS_TEIF) { lowsyslog("DMA receive error\n"); ret = -1; @@ -490,7 +479,6 @@ PX4IO_serial::_wait_complete(bool expect_reply, unsigned tx_length) } /* reset DMA status */ - _tx_dma_status = _dma_status_inactive; _rx_dma_status = _dma_status_inactive; /* update counters */ @@ -507,37 +495,13 @@ PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) if (arg != nullptr) { PX4IO_serial *ps = reinterpret_cast(arg); - if (handle == ps->_tx_dma) { - ps->_do_tx_dma_callback(status); - } else if (handle == ps->_rx_dma) { - ps->_do_rx_dma_callback(status); - } - } -} - -void -PX4IO_serial::_do_tx_dma_callback(unsigned status) -{ - /* on completion of a no-reply transmit, wake the sender */ - if (_tx_dma_status == _dma_status_waiting) { - - /* save TX status */ - _tx_dma_status = status; - - /* disable UART DMA */ - rCR3 &= ~USART_CR3_DMAT; - - /* if we aren't going on to expect a reply, complete now */ - if (_rx_dma_status != _dma_status_waiting) - sem_post(&_completion_semaphore); + ps->_do_rx_dma_callback(status); } } void PX4IO_serial::_do_rx_dma_callback(unsigned status) { - ASSERT(_tx_dma_status != _dma_status_waiting); - /* on completion of a reply, wake the waiter */ if (_rx_dma_status == _dma_status_waiting) { @@ -552,7 +516,7 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) _rx_dma_status = status; /* disable UART DMA */ - rCR3 &= ~USART_CR3_DMAR; + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); /* DMA may have stopped short */ _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); @@ -593,6 +557,10 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); + + /* complete DMA as though in error */ + _do_rx_dma_callback(DMA_STATUS_TEIF); + return; } @@ -605,7 +573,7 @@ PX4IO_serial::_do_interrupt() if (sr & USART_SR_IDLE) { /* if there was DMA transmission still going, this is an error */ - if (_tx_dma_status == _dma_status_waiting) { + if (stm32_dmaresidual(_tx_dma) != 0) { /* babble from IO */ _abort_dma(); @@ -636,13 +604,6 @@ PX4IO_serial::_abort_dma() /* stop DMA */ stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); - - /* complete DMA as though in error */ - _do_tx_dma_callback(DMA_STATUS_TEIF); - _do_rx_dma_callback(DMA_STATUS_TEIF); - - _tx_dma_status = _dma_status_inactive; - _rx_dma_status = _dma_status_inactive; } static const uint8_t crc8_tab[256] = -- cgit v1.2.3 From 1f7f7862ceee827c1c3a6087defb8cb52db3f4c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 17:53:55 -0700 Subject: Fix the USART6 default baudrate to match the IO bootloader. --- nuttx/configs/px4fmuv2/nsh/defconfig | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig index 16be7cd41..17803c4d7 100755 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ b/nuttx/configs/px4fmuv2/nsh/defconfig @@ -188,6 +188,7 @@ CONFIG_STM32_PWR=y CONFIG_STM32_TIM1=y CONFIG_STM32_TIM8=n CONFIG_STM32_USART1=y +# Mostly owned by the px4io driver, but uploader needs this CONFIG_STM32_USART6=y # We use our own driver, but leave this on. CONFIG_STM32_ADC1=y @@ -260,7 +261,7 @@ CONFIG_USART1_BAUD=115200 CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 CONFIG_UART4_BAUD=115200 -CONFIG_USART6_BAUD=9600 +CONFIG_USART6_BAUD=115200 CONFIG_UART7_BAUD=115200 CONFIG_UART8_BAUD=57600 -- cgit v1.2.3 From 46a4a443210b73be01da5d63f9cef955658347ee Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 18:36:00 -0700 Subject: Be more consistent with the packet format definition. Free perf counters in ~PX4IO_serial --- src/drivers/px4io/interface_serial.cpp | 18 ++++++++++++------ src/modules/px4iofirmware/serial.c | 10 +++++----- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 1af64379b..a603f87d6 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -69,7 +69,7 @@ #include "interface.h" -const unsigned max_rw_regs = 32; // by agreement w/IO +#define PKT_MAX_REGS 32 // by agreement w/IO #pragma pack(push, 1) struct IOPacket { @@ -77,7 +77,7 @@ struct IOPacket { uint8_t crc; uint8_t page; uint8_t offset; - uint16_t regs[max_rw_regs]; + uint16_t regs[PKT_MAX_REGS]; }; #pragma pack(pop) @@ -268,6 +268,12 @@ PX4IO_serial::~PX4IO_serial() sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); + perf_free(_perf_dmasetup); + perf_free(_perf_timeouts); + perf_free(_perf_errors); + perf_free(_perf_txns); + perf_free(_perf_crcerrs); + if (g_interface == this) g_interface = nullptr; } @@ -334,7 +340,7 @@ PX4IO_serial::test(unsigned mode) int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - if (num_values > max_rw_regs) + if (num_values > PKT_MAX_REGS) return -EINVAL; sem_wait(&_bus_semaphore); @@ -343,7 +349,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi _dma_buffer.page = page; _dma_buffer.offset = offset; memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < max_rw_regs; i++) + for (unsigned i = num_values; i < PKT_MAX_REGS; i++) _dma_buffer.regs[i] = 0x55aa; /* XXX implement check byte */ @@ -358,7 +364,7 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - if (num_values > max_rw_regs) + if (num_values > PKT_MAX_REGS) return -EINVAL; sem_wait(&_bus_semaphore); @@ -422,7 +428,7 @@ PX4IO_serial::_wait_complete() _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), /* XXX should be tx_length */ + sizeof(_dma_buffer), /* XXX should be PKT_LENGTH() */ DMA_SCR_DIR_M2P | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 6c6a9a2b1..38cfd3ccf 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -77,7 +77,7 @@ static void dma_reset(void); /* if we spend this many ticks idle, reset the DMA */ static unsigned idle_ticks; -#define MAX_RW_REGS 32 // by agreement w/FMU +#define PKT_MAX_REGS 32 // by agreement w/FMU #pragma pack(push, 1) struct IOPacket { @@ -85,7 +85,7 @@ struct IOPacket { uint8_t crc; uint8_t page; uint8_t offset; - uint16_t regs[MAX_RW_REGS]; + uint16_t regs[PKT_MAX_REGS]; }; #pragma pack(pop) @@ -231,8 +231,8 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) count = 0; /* constrain reply to packet size */ - if (count > MAX_RW_REGS) - count = MAX_RW_REGS; + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count); @@ -251,7 +251,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) tx_dma, (uint32_t)&rDR, (uint32_t)&dma_packet, - sizeof(dma_packet), /* XXX cut back to actual transmit size */ + sizeof(dma_packet), /* XXX should be PKT_LENGTH() */ DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | -- cgit v1.2.3 From 10e673aa4b16a7b50656962b4ead7fa87fa94d59 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 19:02:42 -0700 Subject: Send error response if register write fails. --- src/modules/px4iofirmware/serial.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 38cfd3ccf..e170d5bdf 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -216,8 +216,12 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ - if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } } else { -- cgit v1.2.3 From 87c3d1a8c14e9d97bb98d8255c1ba35e875b6c81 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 19:03:08 -0700 Subject: More link performance counters. --- src/drivers/px4io/interface_serial.cpp | 48 +++++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index a603f87d6..2ba251b5f 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -175,9 +175,10 @@ private: */ perf_counter_t _perf_dmasetup; perf_counter_t _perf_timeouts; - perf_counter_t _perf_errors; - perf_counter_t _perf_txns; perf_counter_t _perf_crcerrs; + perf_counter_t _perf_dmaerrs; + perf_counter_t _perf_protoerrs; + perf_counter_t _perf_txns; }; @@ -194,11 +195,12 @@ PX4IO_serial::PX4IO_serial() : _rx_dma(nullptr), _rx_length(0), _rx_dma_status(_dma_status_inactive), - _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup")), - _perf_timeouts(perf_alloc(PC_COUNT, "timeouts")), - _perf_errors(perf_alloc(PC_COUNT, "errors ")), - _perf_txns(perf_alloc(PC_ELAPSED, "txns ")), - _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")) + _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _perf_timeouts(perf_alloc(PC_COUNT, "timeouts ")), + _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), + _perf_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), + _perf_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -270,9 +272,10 @@ PX4IO_serial::~PX4IO_serial() perf_free(_perf_dmasetup); perf_free(_perf_timeouts); - perf_free(_perf_errors); perf_free(_perf_txns); perf_free(_perf_crcerrs); + perf_free(_perf_dmaerrs); + perf_free(_perf_protoerrs); if (g_interface == this) g_interface = nullptr; @@ -320,10 +323,11 @@ PX4IO_serial::test(unsigned mode) if (count > 100) { perf_print_counter(_perf_dmasetup); - perf_print_counter(_perf_txns); perf_print_counter(_perf_timeouts); - perf_print_counter(_perf_errors); + perf_print_counter(_perf_txns); perf_print_counter(_perf_crcerrs); + perf_print_counter(_perf_dmaerrs); + perf_print_counter(_perf_protoerrs); count = 0; } usleep(10000); @@ -461,26 +465,42 @@ PX4IO_serial::_wait_complete() if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { - lowsyslog("DMA receive error\n"); + perf_count(_perf_dmaerrs); ret = -1; + errno = EIO; break; } /* check packet CRC */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; - if (crc != crc_packet(_dma_buffer)) + if (crc != crc_packet(_dma_buffer)) { perf_count(_perf_crcerrs); + ret = -1; + errno = EIO; + break; + } + + /* check packet response code */ + if (PKT_CODE(_dma_buffer) != PKT_CODE_SUCCESS) { + perf_count(_perf_protoerrs); + ret = -1; + errno = EIO; + break; + } + + /* successful txn */ break; } if (errno == ETIMEDOUT) { - lowsyslog("timeout waiting for PX4IO link (%d/%d)\n", stm32_dmaresidual(_tx_dma), stm32_dmaresidual(_rx_dma)); /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); perf_count(_perf_timeouts); break; } + + /* we might? see this for EINTR */ lowsyslog("unexpected ret %d/%d\n", ret, errno); } @@ -489,8 +509,6 @@ PX4IO_serial::_wait_complete() /* update counters */ perf_end(_perf_txns); - if (ret != OK) - perf_count(_perf_errors); return ret; } -- cgit v1.2.3 From f9a85ac7e64ca816253e94a473e2ef04f2962ab5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 19:16:25 -0700 Subject: Remove the TX completion callback on the IO side. Report CRC, read and protocol errors. --- src/modules/px4iofirmware/serial.c | 89 +++++++++++++++++++++----------------- 1 file changed, 49 insertions(+), 40 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e170d5bdf..665a7622c 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,8 +56,6 @@ //#define DEBUG #include "px4io.h" -static volatile bool sending = false; - static perf_counter_t pc_rx; static perf_counter_t pc_errors; static perf_counter_t pc_ore; @@ -66,8 +64,8 @@ static perf_counter_t pc_ne; static perf_counter_t pc_regerr; static perf_counter_t pc_crcerr; +static void rx_handle_packet(void); static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); -static void tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static DMA_HANDLE tx_dma; static DMA_HANDLE rx_dma; @@ -186,33 +184,24 @@ interface_tick() } static void -tx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) -{ - sending = false; - rCR3 &= ~USART_CR3_DMAT; -} - -static void -rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +rx_handle_packet(void) { - /* we just received a request; sort out what to do */ - - rCR3 &= ~USART_CR3_DMAR; - idle_ticks = 0; - - /* work out how big the packet actually is */ - //unsigned rx_length = sizeof(IOPacket) - stm32_dmaresidual(rx_dma); - - /* XXX implement check byte */ - perf_count(pc_rx); + /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; - if (crc != crc_packet()) + if (crc != crc_packet()) { perf_count(pc_crcerr); - /* default to not sending a reply */ + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ @@ -222,33 +211,54 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) } else { dma_packet.count_code = PKT_CODE_SUCCESS; } + return; + } - } else { + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { /* it's a read - get register pointer for reply */ - int result; unsigned count; uint16_t *registers; - result = registers_get(dma_packet.page, dma_packet.offset, ®isters, &count); - if (result < 0) - count = 0; + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; + if (count > PKT_COUNT(dma_packet)) + count = PKT_COUNT(dma_packet); + + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + return; + } - /* constrain reply to packet size */ - if (count > PKT_MAX_REGS) - count = PKT_MAX_REGS; + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} - /* copy reply registers into DMA buffer */ - memcpy((void *)&dma_packet.regs[0], registers, count); - dma_packet.count_code = count | PKT_CODE_SUCCESS; - } +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* reset the idle counter */ + idle_ticks = 0; + + /* handle the received packet */ + rx_handle_packet(); /* re-set DMA for reception first, so we are ready to receive before we start sending */ - /* XXX latency here could mean loss of back-to-back writes; do we want to always send an ack? */ - /* XXX always sending an ack would simplify the FMU side (always wait for reply) too */ dma_reset(); - /* if we have a reply to send, start that now */ + /* send the reply to the previous request */ dma_packet.crc = 0; dma_packet.crc = crc_packet(); stm32_dmasetup( @@ -260,8 +270,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | DMA_CCR_MSIZE_8BITS); - sending = true; - stm32_dmastart(tx_dma, tx_dma_callback, NULL, false); + stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; } -- cgit v1.2.3 From bcfb713fe9f123db6d594315465b30dc7210be75 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 20:35:55 -0700 Subject: Enable handling for short-packet reception on IO using the line-idle interrupt from the UART. --- src/modules/px4iofirmware/serial.c | 37 ++++++++++++++++++++++--------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 665a7622c..21ecde727 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -56,11 +56,13 @@ //#define DEBUG #include "px4io.h" -static perf_counter_t pc_rx; +static perf_counter_t pc_txns; static perf_counter_t pc_errors; static perf_counter_t pc_ore; static perf_counter_t pc_fe; static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; static perf_counter_t pc_regerr; static perf_counter_t pc_crcerr; @@ -117,11 +119,13 @@ static struct IOPacket dma_packet; void interface_init(void) { - pc_rx = perf_alloc(PC_COUNT, "rx count"); + pc_txns = perf_alloc(PC_ELAPSED, "txns"); pc_errors = perf_alloc(PC_COUNT, "errors"); pc_ore = perf_alloc(PC_COUNT, "overrun"); pc_fe = perf_alloc(PC_COUNT, "framing"); pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); pc_regerr = perf_alloc(PC_COUNT, "regerr"); pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); @@ -154,7 +158,7 @@ interface_init(void) /* enable UART and DMA */ /*rCR3 = USART_CR3_EIE; */ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ for (;;) { @@ -186,8 +190,6 @@ interface_tick() static void rx_handle_packet(void) { - perf_count(pc_rx); - /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; @@ -246,6 +248,8 @@ rx_handle_packet(void) static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { + perf_begin(pc_txns); + /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); @@ -272,21 +276,17 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) DMA_CCR_MSIZE_8BITS); stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; + + perf_end(pc_txns); } static int serial_interrupt(int irq, void *context) { uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ - /* handle error/exception conditions */ - if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { - /* read DR to clear status */ - (void)rDR; - } else { - ASSERT(0); - } - +#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -305,10 +305,17 @@ serial_interrupt(int irq, void *context) /* don't attempt to handle IDLE if it's set - things went bad */ return 0; } - +#endif if (sr & USART_SR_IDLE) { - /* XXX if there was DMA transmission still going, this is an error */ + /* the packet might have been short - go check */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + perf_count(pc_badidle); + return 0; + } + + perf_count(pc_idle); /* stop the receive DMA */ stm32_dmastop(rx_dma); -- cgit v1.2.3 From 3c8c596ac7a2eacc3f4ac047a58bd5dceb36a203 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 20:37:05 -0700 Subject: Enable handling for short-packet reception on FMU using the line-idle interrupt from the UART. Enable short packets at both ends. --- src/drivers/px4io/interface_serial.cpp | 124 ++++++++++++++++++--------------- src/modules/px4iofirmware/serial.c | 2 +- 2 files changed, 67 insertions(+), 59 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 2ba251b5f..09362fe15 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -135,7 +135,6 @@ private: DMA_HANDLE _tx_dma; DMA_HANDLE _rx_dma; - volatile unsigned _rx_length; /** saved DMA status */ static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values @@ -173,12 +172,14 @@ private: /** * Performance counters. */ - perf_counter_t _perf_dmasetup; - perf_counter_t _perf_timeouts; - perf_counter_t _perf_crcerrs; - perf_counter_t _perf_dmaerrs; - perf_counter_t _perf_protoerrs; - perf_counter_t _perf_txns; + perf_counter_t _pc_dmasetup; + perf_counter_t _pc_timeouts; + perf_counter_t _pc_crcerrs; + perf_counter_t _pc_dmaerrs; + perf_counter_t _pc_protoerrs; + perf_counter_t _pc_idle; + perf_counter_t _pc_badidle; + perf_counter_t _pc_txns; }; @@ -193,14 +194,15 @@ PX4IO_interface *io_serial_interface() PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), - _rx_length(0), _rx_dma_status(_dma_status_inactive), - _perf_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), - _perf_timeouts(perf_alloc(PC_COUNT, "timeouts ")), - _perf_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), - _perf_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), - _perf_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), - _perf_txns(perf_alloc(PC_ELAPSED, "txns ")) + _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _pc_idle(perf_alloc(PC_COUNT, "idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "badidle ")), + _pc_txns(perf_alloc(PC_ELAPSED, "txns ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -233,7 +235,7 @@ PX4IO_serial::PX4IO_serial() : /* enable UART in DMA mode, enable error and line idle interrupts */ /* rCR3 = USART_CR3_EIE;*/ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE /*| USART_CR1_IDLEIE*/; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ sem_init(&_completion_semaphore, 0, 0); @@ -270,12 +272,14 @@ PX4IO_serial::~PX4IO_serial() sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); - perf_free(_perf_dmasetup); - perf_free(_perf_timeouts); - perf_free(_perf_txns); - perf_free(_perf_crcerrs); - perf_free(_perf_dmaerrs); - perf_free(_perf_protoerrs); + perf_free(_pc_dmasetup); + perf_free(_pc_timeouts); + perf_free(_pc_crcerrs); + perf_free(_pc_dmaerrs); + perf_free(_pc_protoerrs); + perf_free(_pc_idle); + perf_free(_pc_badidle); + perf_free(_pc_txns); if (g_interface == this) g_interface = nullptr; @@ -321,13 +325,15 @@ PX4IO_serial::test(unsigned mode) set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1); /* ignore errors */ - if (count > 100) { - perf_print_counter(_perf_dmasetup); - perf_print_counter(_perf_timeouts); - perf_print_counter(_perf_txns); - perf_print_counter(_perf_crcerrs); - perf_print_counter(_perf_dmaerrs); - perf_print_counter(_perf_protoerrs); + if (count >= 100) { + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + perf_print_counter(_pc_txns); count = 0; } usleep(10000); @@ -404,17 +410,27 @@ PX4IO_serial::_wait_complete() (void)rDR; /* start RX DMA */ - perf_begin(_perf_txns); - perf_begin(_perf_dmasetup); + perf_begin(_pc_txns); + perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; - _rx_length = 0; + + /* + * Note that we enable circular buffer mode as a workaround for + * there being no API to disable the DMA FIFO. We need direct mode + * because otherwise when the line idle interrupt fires there + * will be packet bytes still in the DMA FIFO, and we will assume + * that the idle was spurious. + * + * XXX this should be fixed with a NuttX change. + */ stm32_dmasetup( _rx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast(&_dma_buffer), sizeof(_dma_buffer), + DMA_SCR_CIRC | /* XXX see note above */ DMA_SCR_DIR_P2M | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | @@ -432,7 +448,7 @@ PX4IO_serial::_wait_complete() _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), /* XXX should be PKT_LENGTH() */ + PKT_SIZE(_dma_buffer), DMA_SCR_DIR_M2P | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | @@ -442,7 +458,7 @@ PX4IO_serial::_wait_complete() stm32_dmastart(_tx_dma, nullptr, nullptr, false); rCR3 |= USART_CR3_DMAT; - perf_end(_perf_dmasetup); + perf_end(_pc_dmasetup); /* compute the deadline for a 5ms timeout */ struct timespec abstime; @@ -465,7 +481,7 @@ PX4IO_serial::_wait_complete() if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { - perf_count(_perf_dmaerrs); + perf_count(_pc_dmaerrs); ret = -1; errno = EIO; break; @@ -475,7 +491,7 @@ PX4IO_serial::_wait_complete() uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; if (crc != crc_packet(_dma_buffer)) { - perf_count(_perf_crcerrs); + perf_count(_pc_crcerrs); ret = -1; errno = EIO; break; @@ -483,7 +499,7 @@ PX4IO_serial::_wait_complete() /* check packet response code */ if (PKT_CODE(_dma_buffer) != PKT_CODE_SUCCESS) { - perf_count(_perf_protoerrs); + perf_count(_pc_protoerrs); ret = -1; errno = EIO; break; @@ -496,7 +512,7 @@ PX4IO_serial::_wait_complete() if (errno == ETIMEDOUT) { /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); - perf_count(_perf_timeouts); + perf_count(_pc_timeouts); break; } @@ -508,7 +524,7 @@ PX4IO_serial::_wait_complete() _rx_dma_status = _dma_status_inactive; /* update counters */ - perf_end(_perf_txns); + perf_end(_pc_txns); return ret; } @@ -542,9 +558,6 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status) /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - /* DMA may have stopped short */ - _rx_length = sizeof(IOPacket) - stm32_dmaresidual(_rx_dma); - /* complete now */ sem_post(&_completion_semaphore); } @@ -562,15 +575,9 @@ void PX4IO_serial::_do_interrupt() { uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* read DR to clear status */ - /* handle error/exception conditions */ - if (sr & (USART_SR_ORE | USART_SR_NE | USART_SR_FE | USART_SR_IDLE)) { - /* read DR to clear status */ - (void)rDR; - } else { - ASSERT(0); - } - +#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -593,20 +600,21 @@ PX4IO_serial::_do_interrupt() /* don't attempt to handle IDLE if it's set - things went bad */ return; } - +#endif if (sr & USART_SR_IDLE) { - /* if there was DMA transmission still going, this is an error */ - if (stm32_dmaresidual(_tx_dma) != 0) { - - /* babble from IO */ - _abort_dma(); - return; - } - /* if there is DMA reception going on, this is a short packet */ if (_rx_dma_status == _dma_status_waiting) { + /* verify that the received packet is complete */ + unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { + perf_count(_pc_badidle); + return; + } + + perf_count(_pc_idle); + /* stop the receive DMA */ stm32_dmastop(_rx_dma); diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 21ecde727..b91819373 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -269,7 +269,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) tx_dma, (uint32_t)&rDR, (uint32_t)&dma_packet, - sizeof(dma_packet), /* XXX should be PKT_LENGTH() */ + PKT_SIZE(dma_packet), DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | -- cgit v1.2.3 From 6871d2909b5be7eb93bf23aea771a86aa1b0ae3f Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 22:53:57 -0700 Subject: Add a mechanism for cancelling begin/end perf counters. --- src/modules/systemlib/perf_counter.c | 41 ++++++++++++++++++++++++++++++------ src/modules/systemlib/perf_counter.h | 14 +++++++++++- 2 files changed, 47 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 879f4715a..3c1e10287 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -201,23 +201,50 @@ perf_end(perf_counter_t handle) switch (handle->type) { case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - pce->event_count++; - pce->time_total += elapsed; + if (pce->time_start != 0) { + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - if ((pce->time_least > elapsed) || (pce->time_least == 0)) - pce->time_least = elapsed; + pce->event_count++; + pce->time_total += elapsed; - if (pce->time_most < elapsed) - pce->time_most = elapsed; + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + + pce->time_start = 0; + } + } + break; + + default: + break; + } +} + +void +perf_cancel(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + pce->time_start = 0; } + break; default: break; } } + + void perf_reset(perf_counter_t handle) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 5c2cb15b2..4cd8b67a1 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -92,13 +92,25 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. */ __EXPORT extern void perf_end(perf_counter_t handle); /** - * Reset a performance event. + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_cancel(perf_counter_t handle); + +/** + * Reset a performance counter. * * This call resets performance counter to initial state * -- cgit v1.2.3 From a4b0e3ecbe2d012eac7545cce14829866bacc813 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 5 Jul 2013 22:54:44 -0700 Subject: Add retry-on-error for non-protocol errors. Add more performance counters; run test #1 faster. --- src/drivers/px4io/interface_serial.cpp | 150 +++++++++++++++++++++------------ 1 file changed, 98 insertions(+), 52 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 09362fe15..7e4a54ea5 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -172,14 +172,16 @@ private: /** * Performance counters. */ + perf_counter_t _pc_txns; perf_counter_t _pc_dmasetup; + perf_counter_t _pc_retries; perf_counter_t _pc_timeouts; perf_counter_t _pc_crcerrs; perf_counter_t _pc_dmaerrs; perf_counter_t _pc_protoerrs; + perf_counter_t _pc_uerrs; perf_counter_t _pc_idle; perf_counter_t _pc_badidle; - perf_counter_t _pc_txns; }; @@ -195,14 +197,16 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), + _pc_txns(perf_alloc(PC_ELAPSED, "txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "uarterrs ")), _pc_idle(perf_alloc(PC_COUNT, "idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "badidle ")), - _pc_txns(perf_alloc(PC_ELAPSED, "txns ")) + _pc_badidle(perf_alloc(PC_COUNT, "badidle ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); @@ -272,14 +276,16 @@ PX4IO_serial::~PX4IO_serial() sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); + perf_free(_pc_txns); perf_free(_pc_dmasetup); + perf_free(_pc_retries); perf_free(_pc_timeouts); perf_free(_pc_crcerrs); perf_free(_pc_dmaerrs); perf_free(_pc_protoerrs); + perf_free(_pc_uerrs); perf_free(_pc_idle); perf_free(_pc_badidle); - perf_free(_pc_txns); if (g_interface == this) g_interface = nullptr; @@ -317,26 +323,29 @@ PX4IO_serial::test(unsigned mode) return 0; case 1: - lowsyslog("test 1\n"); { + unsigned fails = 0; for (unsigned count = 0;; count++) { uint16_t value = count & 0xffff; - set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1); - /* ignore errors */ + if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; - if (count >= 100) { + if (count >= 1000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); perf_print_counter(_pc_dmaerrs); perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); perf_print_counter(_pc_idle); perf_print_counter(_pc_badidle); - perf_print_counter(_pc_txns); count = 0; } - usleep(10000); + usleep(1000); } return 0; } @@ -350,22 +359,44 @@ PX4IO_serial::test(unsigned mode) int PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - if (num_values > PKT_MAX_REGS) - return -EINVAL; + if (num_values > PKT_MAX_REGS) { + errno = EINVAL; + return -1; + } sem_wait(&_bus_semaphore); - _dma_buffer.count_code = num_values | PKT_CODE_WRITE; - _dma_buffer.page = page; - _dma_buffer.offset = offset; - memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < PKT_MAX_REGS; i++) - _dma_buffer.regs[i] = 0x55aa; + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = num_values | PKT_CODE_WRITE; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + for (unsigned i = num_values; i < PKT_MAX_REGS; i++) + _dma_buffer.regs[i] = 0x55aa; + + /* XXX implement check byte */ - /* XXX implement check byte */ + /* start the transaction and wait for it to complete */ + result = _wait_complete(); - /* start the transaction and wait for it to complete */ - int result = _wait_complete(); + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + errno = EINVAL; + result = -1; + perf_count(_pc_protoerrs); + } + + break; + } + perf_count(_pc_retries); + } sem_post(&_bus_semaphore); return result; @@ -379,23 +410,45 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n sem_wait(&_bus_semaphore); - _dma_buffer.count_code = num_values | PKT_CODE_READ; - _dma_buffer.page = page; - _dma_buffer.offset = offset; + int result; + for (unsigned retries = 0; retries < 3; retries++) { - /* start the transaction and wait for it to complete */ - int result = _wait_complete(); - if (result != OK) - goto out; + _dma_buffer.count_code = num_values | PKT_CODE_READ; + _dma_buffer.page = page; + _dma_buffer.offset = offset; - /* compare the received count with the expected count */ - if (PKT_COUNT(_dma_buffer) != num_values) { - result = -EIO; - goto out; - } else { - /* XXX implement check byte */ - /* copy back the result */ - memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + errno = EINVAL; + result = -1; + perf_count(_pc_protoerrs); + + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != num_values) { + + /* IO returned the wrong number of registers - no point retrying */ + errno = EIO; + result = -1; + perf_count(_pc_protoerrs); + + /* successful read */ + } else { + + /* copy back the result */ + memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + } + + break; + } + perf_count(_pc_retries); } out: sem_post(&_bus_semaphore); @@ -463,11 +516,11 @@ PX4IO_serial::_wait_complete() /* compute the deadline for a 5ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); -#if 1 +#if 0 abstime.tv_sec++; /* long timeout for testing */ #else - abstime.tv_nsec += 5000000; /* 5ms timeout */ - while (abstime.tv_nsec > 1000000000) { + abstime.tv_nsec += 10000000; /* 0ms timeout */ + if (abstime.tv_nsec > 1000000000) { abstime.tv_sec++; abstime.tv_nsec -= 1000000000; } @@ -487,25 +540,17 @@ PX4IO_serial::_wait_complete() break; } - /* check packet CRC */ + /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; - if (crc != crc_packet(_dma_buffer)) { + if ((crc != crc_packet(_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -1; errno = EIO; break; } - /* check packet response code */ - if (PKT_CODE(_dma_buffer) != PKT_CODE_SUCCESS) { - perf_count(_pc_protoerrs); - ret = -1; - errno = EIO; - break; - } - - /* successful txn */ + /* successful txn (may still be reporting an error) */ break; } @@ -513,6 +558,7 @@ PX4IO_serial::_wait_complete() /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ break; } @@ -577,7 +623,6 @@ PX4IO_serial::_do_interrupt() uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* read DR to clear status */ -#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -588,6 +633,7 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); + perf_count(_pc_uerrs); /* complete DMA as though in error */ _do_rx_dma_callback(DMA_STATUS_TEIF); @@ -600,7 +646,7 @@ PX4IO_serial::_do_interrupt() /* don't attempt to handle IDLE if it's set - things went bad */ return; } -#endif + if (sr & USART_SR_IDLE) { /* if there is DMA reception going on, this is a short packet */ -- cgit v1.2.3 From 0589346ce6ccbcee03437ee293cc03d900bf76d9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 00:00:10 -0700 Subject: Abort the px4io worker task if subscribing to the required ORB topics fails. --- src/drivers/px4io/px4io.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 663609aed..5895a4eb5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -572,6 +572,14 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + if ((_t_actuators < 0) || + (_t_armed < 0) || + (_t_vstatus < 0) || + (_t_param < 0)) { + log("subscription(s) failed"); + goto out; + } + /* poll descriptor */ pollfd fds[4]; fds[0].fd = _t_actuators; @@ -668,6 +676,7 @@ PX4IO::task_main() unlock(); +out: debug("exiting"); /* clean up the alternate device node */ -- cgit v1.2.3 From 19b2e1de8505be6ab23bedab7b105a20ac7af405 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 00:00:44 -0700 Subject: Copy the correct number of bytes back for register read operations. Basic PX4IO comms are working now. --- src/modules/px4iofirmware/serial.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index b91819373..f19021d8c 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -233,7 +233,7 @@ rx_handle_packet(void) count = PKT_COUNT(dma_packet); /* copy reply registers into DMA buffer */ - memcpy((void *)&dma_packet.regs[0], registers, count); + memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } return; -- cgit v1.2.3 From 87a4f1507a3bca4bcae870b13ff5416669ededf0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 00:16:37 -0700 Subject: Move the common definitions for the PX4IO serial protocol into the shared header. --- src/drivers/px4io/interface_serial.cpp | 81 +--------------------------------- src/modules/px4iofirmware/protocol.h | 78 ++++++++++++++++++++++++++++++++ src/modules/px4iofirmware/serial.c | 79 +-------------------------------- 3 files changed, 82 insertions(+), 156 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 7e4a54ea5..7e0eb1926 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -69,18 +69,6 @@ #include "interface.h" -#define PKT_MAX_REGS 32 // by agreement w/IO - -#pragma pack(push, 1) -struct IOPacket { - uint8_t count_code; - uint8_t crc; - uint8_t page; - uint8_t offset; - uint16_t regs[PKT_MAX_REGS]; -}; -#pragma pack(pop) - /* serial register accessors */ #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) #define rSR REG(STM32_USART_SR_OFFSET) @@ -91,22 +79,6 @@ struct IOPacket { #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ -#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ -#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ -#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ -#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ - -#define PKT_CODE_MASK 0xc0 -#define PKT_COUNT_MASK 0x3f - -#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) -#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) - -static uint8_t crc_packet(IOPacket &pkt); - - class PX4IO_serial : public PX4IO_interface { public: @@ -496,7 +468,7 @@ PX4IO_serial::_wait_complete() /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ _dma_buffer.crc = 0; - _dma_buffer.crc = crc_packet(_dma_buffer); + _dma_buffer.crc = crc_packet(&_dma_buffer); stm32_dmasetup( _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, @@ -543,7 +515,7 @@ PX4IO_serial::_wait_complete() /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _dma_buffer.crc; _dma_buffer.crc = 0; - if ((crc != crc_packet(_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -1; errno = EIO; @@ -683,52 +655,3 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); } - -static const uint8_t crc8_tab[256] = -{ - 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, - 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, - 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, - 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, - 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, - 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, - 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, - 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, - 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, - 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, - 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, - 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, - 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, - 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, - 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, - 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, - 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, - 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, - 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, - 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, - 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, - 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, - 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, - 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, - 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, - 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, - 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, - 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, - 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, - 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, - 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, - 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 -}; - -static uint8_t -crc_packet(IOPacket &pkt) -{ - uint8_t *end = (uint8_t *)(&pkt.regs[PKT_COUNT(pkt)]); - uint8_t *p = (uint8_t *)&pkt; - uint8_t c = 0; - - while (p < end) - c = crc8_tab[c ^ *(p++)]; - - return c; -} diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b19146b06..48ad4316f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -206,3 +206,81 @@ struct px4io_mixdata { }; #pragma pack(pop) +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index f19021d8c..8742044c3 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -77,33 +77,6 @@ static void dma_reset(void); /* if we spend this many ticks idle, reset the DMA */ static unsigned idle_ticks; -#define PKT_MAX_REGS 32 // by agreement w/FMU - -#pragma pack(push, 1) -struct IOPacket { - uint8_t count_code; - uint8_t crc; - uint8_t page; - uint8_t offset; - uint16_t regs[PKT_MAX_REGS]; -}; -#pragma pack(pop) - -#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ -#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ -#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ -#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ -#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ - -#define PKT_CODE_MASK 0xc0 -#define PKT_COUNT_MASK 0x3f - -#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) -#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) - -static uint8_t crc_packet(void); - static struct IOPacket dma_packet; /* serial register accessors */ @@ -193,7 +166,7 @@ rx_handle_packet(void) /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; - if (crc != crc_packet()) { + if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); /* send a CRC error reply */ @@ -264,7 +237,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* send the reply to the previous request */ dma_packet.crc = 0; - dma_packet.crc = crc_packet(); + dma_packet.crc = crc_packet(&dma_packet); stm32_dmasetup( tx_dma, (uint32_t)&rDR, @@ -354,51 +327,3 @@ dma_reset(void) rCR3 |= USART_CR3_DMAR; } -static const uint8_t crc8_tab[256] = -{ - 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, - 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, - 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, - 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, - 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, - 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, - 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, - 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, - 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, - 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, - 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, - 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, - 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, - 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, - 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, - 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, - 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, - 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, - 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, - 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, - 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, - 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, - 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, - 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, - 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, - 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, - 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, - 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, - 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, - 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, - 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, - 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 -}; - -static uint8_t -crc_packet() -{ - uint8_t *end = (uint8_t *)(&dma_packet.regs[PKT_COUNT(dma_packet)]); - uint8_t *p = (uint8_t *)&dma_packet; - uint8_t c = 0; - - while (p < end) - c = crc8_tab[c ^ *(p++)]; - - return c; -} -- cgit v1.2.3 From 4d400aa7e75aa091fb649bbeaf0a2d6a644c177c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 12:27:37 -0700 Subject: Enable UART error handling on PX4IO. --- src/modules/px4iofirmware/serial.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 8742044c3..e4bc68f58 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -129,8 +129,8 @@ interface_init(void) irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); up_enable_irq(PX4FMU_SERIAL_VECTOR); - /* enable UART and DMA */ - /*rCR3 = USART_CR3_EIE; */ + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ @@ -259,7 +259,6 @@ serial_interrupt(int irq, void *context) uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* required to clear any of the interrupt status that brought us here */ -#if 0 if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ @@ -278,7 +277,7 @@ serial_interrupt(int irq, void *context) /* don't attempt to handle IDLE if it's set - things went bad */ return 0; } -#endif + if (sr & USART_SR_IDLE) { /* the packet might have been short - go check */ -- cgit v1.2.3 From 17f9c7d15ccb6301e2be3eaa8cde8b3f710ce085 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 12:28:01 -0700 Subject: Crank up the test speed for 'px4io iftest 1' --- src/drivers/px4io/interface_serial.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 7e0eb1926..9727daa71 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -303,7 +303,7 @@ PX4IO_serial::test(unsigned mode) if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) fails++; - if (count >= 1000) { + if (count >= 5000) { lowsyslog("==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); perf_print_counter(_pc_dmasetup); @@ -317,7 +317,6 @@ PX4IO_serial::test(unsigned mode) perf_print_counter(_pc_badidle); count = 0; } - usleep(1000); } return 0; } -- cgit v1.2.3 From a65a1237f05c885245237e9ffecd79dee9de4dbc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 12:29:14 -0700 Subject: Optimise the RC input fetch for <= 9ch transmitters; this eliminates one read per cycle from IO in the common case. --- src/drivers/px4io/px4io.cpp | 44 ++++++++++++++++++++++---------------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5895a4eb5..951bfe695 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -959,38 +959,38 @@ int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint32_t channel_count; - int ret = OK; + int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; + /* - * XXX Because the channel count and channel data are fetched - * separately, there is a risk of a race between the two - * that could leave us with channel data and a count that - * are out of sync. - * Fixing this would require a guarantee of atomicity from - * IO, and a single fetch for both count and channels. - * - * XXX Since IO has the input calibration info, we ought to be - * able to get the pre-fixed-up controls directly. + * Read the channel count and the first 9 channels. * - * XXX can we do this more cheaply? If we knew we had DMA, it would - * almost certainly be better to just get all the inputs... + * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - if (channel_count == _io_reg_get_error) - return -EIO; - if (channel_count > RC_INPUT_MAX_CHANNELS) - channel_count = RC_INPUT_MAX_CHANNELS; - input_rc.channel_count = channel_count; + input_rc.timestamp = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + if (ret != OK) + return ret; - if (channel_count > 0) { - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); - if (ret == OK) - input_rc.timestamp = hrt_absolute_time(); + /* + * Get the channel count any any extra channels. This is no more expensive than reading the + * channel count once. + */ + channel_count = regs[0]; + if (channel_count > 9) { + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + if (ret != OK) + return ret; } + input_rc.channel_count = channel_count; + memcpy(input_rc.values, ®s[prolog], channel_count * 2); + return ret; } -- cgit v1.2.3 From 8fa226c909d5d34606e8f28bb0b54aeda8f91010 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 23:59:35 -0700 Subject: Tweak protocol register assignments and add new registers to accommodate differences in IOv2. --- src/drivers/px4io/px4io.cpp | 53 ++++++++---- src/modules/px4iofirmware/controls.c | 12 +-- src/modules/px4iofirmware/mixer.cpp | 16 ++-- src/modules/px4iofirmware/protocol.h | 52 +++++++----- src/modules/px4iofirmware/px4io.h | 58 +++++++------ src/modules/px4iofirmware/registers.c | 103 ++++++++++++++++++----- src/systemcmds/preflight_check/preflight_check.c | 2 +- 7 files changed, 198 insertions(+), 98 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 951bfe695..15e213afb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -134,6 +134,7 @@ private: PX4IO_interface *_interface; // XXX + unsigned _hardware; unsigned _max_actuators; unsigned _max_controls; unsigned _max_rc_input; @@ -318,6 +319,7 @@ PX4IO *g_dev; PX4IO::PX4IO(PX4IO_interface *interface) : CDev("px4io", "/dev/px4io"), _interface(interface), + _hardware(0), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -387,18 +389,25 @@ PX4IO::init() return ret; /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); - _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); if ((_max_actuators < 1) || (_max_actuators > 255) || - (_max_relays < 1) || (_max_relays > 255) || + (_max_relays > 32) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { - log("failed getting parameters from PX4IO"); - mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); + log("config read error"); + mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -1122,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned int ret = _interface->set_reg(page, offset, values, num_values); if (ret != OK) - debug("io_reg_set: error %d", ret); + debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); return ret; } @@ -1143,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v int ret = _interface->get_reg(page, offset, values, num_values); if (ret != OK) - debug("io_reg_get: data error %d", ret); + debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); return ret; } @@ -1237,9 +1246,9 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u software %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", @@ -1268,7 +1277,7 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - printf("alarms 0x%04x%s%s%s%s%s%s%s\n", + printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", alarms, ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), @@ -1276,18 +1285,26 @@ PX4IO::print_status() ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); - printf("vbatt %u ibatt %u vbatt scale %u\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); - printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", - (double)_battery_amp_per_volt, - (double)_battery_amp_bias, - (double)_battery_mamphour_total); + if (_hardware == 1) { + printf("vbatt %u ibatt %u vbatt scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); + } else if (_hardware == 2) { + printf("vservo %u vservo scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); + } printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 95103964e..5a95a8aa9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -66,7 +66,7 @@ controls_init(void) sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -117,7 +117,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); /* * In some cases we may have received a frame, but input has still @@ -190,7 +190,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); + ASSERT(mapped < PX4IO_CONTROL_CHANNELS); /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ @@ -202,7 +202,7 @@ controls_tick() { } /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } @@ -312,8 +312,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; + if (*num_values > PX4IO_CONTROL_CHANNELS) + *num_values = PX4IO_CONTROL_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..d8c0e58ba 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,7 +154,7 @@ mixer_tick(void) if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ @@ -164,11 +164,11 @@ mixer_tick(void) } else if (source != MIX_NONE) { - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -180,7 +180,7 @@ mixer_tick(void) r_page_servos[i] = (outputs[i] * 600.0f) + 1500; } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } @@ -215,7 +215,7 @@ mixer_tick(void) if (mixer_servos_armed) { /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); } } @@ -349,11 +349,11 @@ mixer_set_failsafe() return; /* set failsafe defaults to the values for all inputs = 0 */ - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -364,7 +364,7 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servo_failsafe[i] = 0; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 48ad4316f..fa57dfc3f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -62,12 +62,11 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - /* Per C, this is safe for all 2's complement systems */ #define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) @@ -75,10 +74,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PROTOCOL_VERSION 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ @@ -107,16 +108,20 @@ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ -#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -142,7 +147,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -155,18 +160,27 @@ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ + #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* [1] accessory power 2 */ + +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [2] servo voltage correction factor (float) */ + /* 7 */ + /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -178,13 +192,13 @@ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* Debug and test page - not used in normal operation */ -#define PX4IO_PAGE_TEST 127 +#define PX4IO_PAGE_TEST 127 #define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ /** diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0779ffd8f..b32782285 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -54,8 +54,9 @@ /* * Constants and limits. */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 /* * Debug logging @@ -124,34 +125,41 @@ extern struct sys_state_s system_state; /* * GPIO handling. */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IOV2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + +#else /* CONFIG_ARCH_BOARD_PX4IOV1 */ + +# define PX4IO_RELAY_CHANNELS 4 +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VBATT 4 +# define ADC_IN5 5 -#ifdef GPIO_SERVO_PWR_EN -# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC1_PWR_EN -# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC2_PWR_EN -# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#endif -#ifdef GPIO_RELAY1_EN -# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) -#endif -#ifdef GPIO_RELAY2_EN -# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) #endif -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 - /* * Mixer */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index b977375f4..f4541936b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -58,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, +#ifdef CONFIG_ARCH_BOARD_PX4IOV2 + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, +#else + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, +#endif [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -80,7 +84,10 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 + [PX4IO_P_STATUS_IBATT] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0 }; /** @@ -88,14 +95,14 @@ uint16_t r_page_status[] = { * * Post-mixed actuator values. */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; /** * PAGE 3 * * Servo PWM values */ -uint16_t r_page_servos[IO_SERVO_COUNT]; +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; /** * PAGE 4 @@ -105,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -115,7 +122,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -148,7 +155,7 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** @@ -167,7 +174,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -183,7 +190,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -234,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ r_page_servo_failsafe[offset] = *values; @@ -366,6 +373,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #endif break; + case PX4IO_P_SETUP_VBATT_SCALE: + r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; + break; + case PX4IO_P_SETUP_SET_DEBUG: r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); @@ -388,7 +399,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= MAX_CONTROL_CHANNELS) + if (channel >= PX4IO_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -433,7 +444,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { count++; } @@ -497,6 +508,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ +#ifdef ADC_VBATT /* PX4IO_P_STATUS_VBATT */ { /* @@ -530,7 +542,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } } - +#endif +#ifdef ADC_IBATT /* PX4IO_P_STATUS_IBATT */ { /* @@ -540,26 +553,74 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val FMU sort it out, with user selectable configuration for their sensor */ - unsigned counts = adc_measure(ADC_IN5); + unsigned counts = adc_measure(ADC_IBATT); if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * V counts + * 5 1001 + * 6 1219 + * 7 1436 + * 8 1653 + * 9 1870 + * 10 2086 + * 11 2303 + * 12 2522 + * 13 2738 + * 14 2956 + * 15 3172 + * 16 3389 + * + * slope = 0.0046067 + * intercept = 0.3863 + * + * Intercept corrected for best results @ 12V. + */ + unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { + unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + } + } +#endif + /* XXX PX4IO_P_STATUS_VRSSI */ + /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif SELECT_PAGE(r_page_scratch); break; case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); SELECT_PAGE(r_page_scratch); @@ -631,7 +692,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4bcce18fb..6d1ecb321 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -210,7 +210,7 @@ int preflight_check_main(int argc, char *argv[]) } /* XXX needs inspection of all the _MAP params */ - // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); // /* give system time to flush error message in case there are more */ // usleep(100000); -- cgit v1.2.3 From 9fe257c4d151280c770e607bc3160703f9503889 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 12:13:40 -0700 Subject: A very slender config just for test builds. --- ROMFS/px4fmu_test/init.d/rc.standalone | 13 ++++++++++ ROMFS/px4fmu_test/init.d/rcS | 4 +++ makefiles/config_px4fmuv2_test.mk | 45 ++++++++++++++++++++++++++++++++++ 3 files changed, 62 insertions(+) create mode 100644 ROMFS/px4fmu_test/init.d/rc.standalone create mode 100755 ROMFS/px4fmu_test/init.d/rcS create mode 100644 makefiles/config_px4fmuv2_test.mk diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone new file mode 100644 index 000000000..67e95215b --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rc.standalone @@ -0,0 +1,13 @@ +#!nsh +# +# Flight startup script for PX4FMU standalone configuration. +# + +echo "[init] doing standalone PX4FMU startup..." + +# +# Start the ORB +# +uorb start + +echo "[init] startup done" diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS new file mode 100755 index 000000000..7f161b053 --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -0,0 +1,4 @@ +#!nsh +# +# PX4FMU startup script for test hackery. +# diff --git a/makefiles/config_px4fmuv2_test.mk b/makefiles/config_px4fmuv2_test.mk new file mode 100644 index 000000000..0bd6c18e5 --- /dev/null +++ b/makefiles/config_px4fmuv2_test.mk @@ -0,0 +1,45 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmuv2 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot + +# needed because things use it for logging +MODULES += modules/mavlink + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) -- cgit v1.2.3 From b4029dd824cec7a0b53c62e960f80d90ddc6e13c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 17:53:55 -0700 Subject: Pull v2 pieces up to build with the merge --- Images/px4fmu-v2.prototype | 12 + Images/px4fmuv2.prototype | 12 - Images/px4io-v2.prototype | 12 + Makefile | 87 +- makefiles/board_px4fmu-v2.mk | 10 + makefiles/board_px4fmuv2.mk | 10 - makefiles/board_px4io-v2.mk | 10 + makefiles/board_px4iov2.mk | 10 - makefiles/config_px4fmu-v2_default.mk | 122 +++ makefiles/config_px4fmu-v2_test.mk | 40 + makefiles/config_px4fmuv2_default.mk | 122 --- makefiles/config_px4fmuv2_test.mk | 45 - makefiles/config_px4io-v2_default.mk | 10 + makefiles/config_px4iov2_default.mk | 10 - makefiles/setup.mk | 1 - makefiles/upload.mk | 2 +- nuttx-configs/px4fmu-v2/Kconfig | 7 + nuttx-configs/px4fmu-v2/common/Make.defs | 184 ++++ nuttx-configs/px4fmu-v2/common/ld.script | 150 ++++ nuttx-configs/px4fmu-v2/include/board.h | 364 ++++++++ nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h | 42 + nuttx-configs/px4fmu-v2/nsh/Make.defs | 3 + nuttx-configs/px4fmu-v2/nsh/appconfig | 52 ++ nuttx-configs/px4fmu-v2/nsh/defconfig | 1022 ++++++++++++++++++++++ nuttx-configs/px4fmu-v2/nsh/defconfig.prev | 1067 +++++++++++++++++++++++ nuttx-configs/px4fmu-v2/nsh/setenv.sh | 67 ++ nuttx-configs/px4fmu-v2/src/Makefile | 84 ++ nuttx-configs/px4fmu-v2/src/empty.c | 4 + nuttx-configs/px4io-v2/README.txt | 806 ++++++++++++++++++ nuttx-configs/px4io-v2/common/Make.defs | 175 ++++ nuttx-configs/px4io-v2/common/ld.script | 129 +++ nuttx-configs/px4io-v2/common/setenv.sh | 47 + nuttx-configs/px4io-v2/include/README.txt | 1 + nuttx-configs/px4io-v2/include/board.h | 177 ++++ nuttx-configs/px4io-v2/nsh/Make.defs | 3 + nuttx-configs/px4io-v2/nsh/appconfig | 32 + nuttx-configs/px4io-v2/nsh/defconfig | 526 ++++++++++++ nuttx-configs/px4io-v2/nsh/setenv.sh | 47 + nuttx-configs/px4io-v2/src/Makefile | 84 ++ nuttx-configs/px4io-v2/src/README.txt | 1 + nuttx-configs/px4io-v2/src/empty.c | 4 + nuttx-configs/px4io-v2/test/Make.defs | 3 + nuttx-configs/px4io-v2/test/appconfig | 44 + nuttx-configs/px4io-v2/test/defconfig | 544 ++++++++++++ nuttx-configs/px4io-v2/test/setenv.sh | 47 + nuttx/configs/px4fmuv2/Kconfig | 7 - nuttx/configs/px4fmuv2/common/Make.defs | 184 ---- nuttx/configs/px4fmuv2/common/ld.script | 150 ---- nuttx/configs/px4fmuv2/include/board.h | 364 -------- nuttx/configs/px4fmuv2/include/nsh_romfsimg.h | 42 - nuttx/configs/px4fmuv2/nsh/Make.defs | 3 - nuttx/configs/px4fmuv2/nsh/appconfig | 52 -- nuttx/configs/px4fmuv2/nsh/defconfig | 1083 ------------------------ nuttx/configs/px4fmuv2/nsh/setenv.sh | 67 -- nuttx/configs/px4fmuv2/src/Makefile | 84 -- nuttx/configs/px4fmuv2/src/empty.c | 4 - nuttx/configs/px4iov2/README.txt | 806 ------------------ nuttx/configs/px4iov2/common/Make.defs | 175 ---- nuttx/configs/px4iov2/common/ld.script | 129 --- nuttx/configs/px4iov2/common/setenv.sh | 47 - nuttx/configs/px4iov2/include/README.txt | 1 - nuttx/configs/px4iov2/include/board.h | 184 ---- nuttx/configs/px4iov2/io/Make.defs | 3 - nuttx/configs/px4iov2/io/appconfig | 32 - nuttx/configs/px4iov2/io/defconfig | 548 ------------ nuttx/configs/px4iov2/io/setenv.sh | 47 - nuttx/configs/px4iov2/nsh/Make.defs | 3 - nuttx/configs/px4iov2/nsh/appconfig | 44 - nuttx/configs/px4iov2/nsh/defconfig | 567 ------------- nuttx/configs/px4iov2/nsh/setenv.sh | 47 - nuttx/configs/px4iov2/src/Makefile | 84 -- nuttx/configs/px4iov2/src/README.txt | 1 - nuttx/configs/px4iov2/src/empty.c | 4 - src/drivers/boards/px4fmuv2/px4fmu_init.c | 4 +- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 2 +- src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 2 +- src/drivers/boards/px4fmuv2/px4fmu_spi.c | 6 +- src/drivers/boards/px4fmuv2/px4fmu_usb.c | 4 +- src/drivers/boards/px4iov2/px4iov2_init.c | 2 +- src/drivers/boards/px4iov2/px4iov2_internal.h | 2 +- src/drivers/boards/px4iov2/px4iov2_pwm_servo.c | 2 +- src/drivers/drv_gpio.h | 40 +- src/drivers/px4fmu/fmu.cpp | 28 +- src/drivers/px4io/interface_serial.cpp | 1 - src/drivers/px4io/px4io.cpp | 8 +- src/drivers/px4io/uploader.cpp | 4 +- src/drivers/stm32/drv_hrt.c | 8 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 4 - src/modules/px4iofirmware/module.mk | 4 +- src/modules/px4iofirmware/px4io.h | 30 +- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/serial.c | 2 +- 92 files changed, 6044 insertions(+), 5104 deletions(-) create mode 100644 Images/px4fmu-v2.prototype delete mode 100644 Images/px4fmuv2.prototype create mode 100644 Images/px4io-v2.prototype create mode 100644 makefiles/board_px4fmu-v2.mk delete mode 100644 makefiles/board_px4fmuv2.mk create mode 100644 makefiles/board_px4io-v2.mk delete mode 100644 makefiles/board_px4iov2.mk create mode 100644 makefiles/config_px4fmu-v2_default.mk create mode 100644 makefiles/config_px4fmu-v2_test.mk delete mode 100644 makefiles/config_px4fmuv2_default.mk delete mode 100644 makefiles/config_px4fmuv2_test.mk create mode 100644 makefiles/config_px4io-v2_default.mk delete mode 100644 makefiles/config_px4iov2_default.mk create mode 100644 nuttx-configs/px4fmu-v2/Kconfig create mode 100644 nuttx-configs/px4fmu-v2/common/Make.defs create mode 100644 nuttx-configs/px4fmu-v2/common/ld.script create mode 100755 nuttx-configs/px4fmu-v2/include/board.h create mode 100644 nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4fmu-v2/nsh/Make.defs create mode 100644 nuttx-configs/px4fmu-v2/nsh/appconfig create mode 100644 nuttx-configs/px4fmu-v2/nsh/defconfig create mode 100755 nuttx-configs/px4fmu-v2/nsh/defconfig.prev create mode 100755 nuttx-configs/px4fmu-v2/nsh/setenv.sh create mode 100644 nuttx-configs/px4fmu-v2/src/Makefile create mode 100644 nuttx-configs/px4fmu-v2/src/empty.c create mode 100755 nuttx-configs/px4io-v2/README.txt create mode 100644 nuttx-configs/px4io-v2/common/Make.defs create mode 100755 nuttx-configs/px4io-v2/common/ld.script create mode 100755 nuttx-configs/px4io-v2/common/setenv.sh create mode 100755 nuttx-configs/px4io-v2/include/README.txt create mode 100755 nuttx-configs/px4io-v2/include/board.h create mode 100644 nuttx-configs/px4io-v2/nsh/Make.defs create mode 100644 nuttx-configs/px4io-v2/nsh/appconfig create mode 100755 nuttx-configs/px4io-v2/nsh/defconfig create mode 100755 nuttx-configs/px4io-v2/nsh/setenv.sh create mode 100644 nuttx-configs/px4io-v2/src/Makefile create mode 100644 nuttx-configs/px4io-v2/src/README.txt create mode 100644 nuttx-configs/px4io-v2/src/empty.c create mode 100644 nuttx-configs/px4io-v2/test/Make.defs create mode 100644 nuttx-configs/px4io-v2/test/appconfig create mode 100755 nuttx-configs/px4io-v2/test/defconfig create mode 100755 nuttx-configs/px4io-v2/test/setenv.sh delete mode 100644 nuttx/configs/px4fmuv2/Kconfig delete mode 100644 nuttx/configs/px4fmuv2/common/Make.defs delete mode 100644 nuttx/configs/px4fmuv2/common/ld.script delete mode 100755 nuttx/configs/px4fmuv2/include/board.h delete mode 100644 nuttx/configs/px4fmuv2/include/nsh_romfsimg.h delete mode 100644 nuttx/configs/px4fmuv2/nsh/Make.defs delete mode 100644 nuttx/configs/px4fmuv2/nsh/appconfig delete mode 100755 nuttx/configs/px4fmuv2/nsh/defconfig delete mode 100755 nuttx/configs/px4fmuv2/nsh/setenv.sh delete mode 100644 nuttx/configs/px4fmuv2/src/Makefile delete mode 100644 nuttx/configs/px4fmuv2/src/empty.c delete mode 100755 nuttx/configs/px4iov2/README.txt delete mode 100644 nuttx/configs/px4iov2/common/Make.defs delete mode 100755 nuttx/configs/px4iov2/common/ld.script delete mode 100755 nuttx/configs/px4iov2/common/setenv.sh delete mode 100755 nuttx/configs/px4iov2/include/README.txt delete mode 100755 nuttx/configs/px4iov2/include/board.h delete mode 100644 nuttx/configs/px4iov2/io/Make.defs delete mode 100644 nuttx/configs/px4iov2/io/appconfig delete mode 100755 nuttx/configs/px4iov2/io/defconfig delete mode 100755 nuttx/configs/px4iov2/io/setenv.sh delete mode 100644 nuttx/configs/px4iov2/nsh/Make.defs delete mode 100644 nuttx/configs/px4iov2/nsh/appconfig delete mode 100755 nuttx/configs/px4iov2/nsh/defconfig delete mode 100755 nuttx/configs/px4iov2/nsh/setenv.sh delete mode 100644 nuttx/configs/px4iov2/src/Makefile delete mode 100644 nuttx/configs/px4iov2/src/README.txt delete mode 100644 nuttx/configs/px4iov2/src/empty.c diff --git a/Images/px4fmu-v2.prototype b/Images/px4fmu-v2.prototype new file mode 100644 index 000000000..5109b77d1 --- /dev/null +++ b/Images/px4fmu-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 9, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv2 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv2", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Images/px4fmuv2.prototype b/Images/px4fmuv2.prototype deleted file mode 100644 index 5109b77d1..000000000 --- a/Images/px4fmuv2.prototype +++ /dev/null @@ -1,12 +0,0 @@ -{ - "board_id": 9, - "magic": "PX4FWv1", - "description": "Firmware for the PX4FMUv2 board", - "image": "", - "build_time": 0, - "summary": "PX4FMUv2", - "version": "0.1", - "image_size": 0, - "git_identity": "", - "board_revision": 0 -} diff --git a/Images/px4io-v2.prototype b/Images/px4io-v2.prototype new file mode 100644 index 000000000..af87924e9 --- /dev/null +++ b/Images/px4io-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Makefile b/Makefile index 7f98ffaf2..9905f8a63 100644 --- a/Makefile +++ b/Makefile @@ -100,7 +100,7 @@ all: $(STAGED_FIRMWARES) # is taken care of. # $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 - @echo %% Copying $@ + @$(ECHO) %% Copying $@ $(Q) $(COPY) $< $@ $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) @@ -111,9 +111,9 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: - @echo %%%% - @echo %%%% Building $(config) in $(work_dir) - @echo %%%% + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% $(Q) mkdir -p $(work_dir) $(Q) make -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ @@ -132,8 +132,6 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: # XXX Should support fetching/unpacking from a separate directory to permit # downloads of the prebuilt archives as well... # -# XXX PX4IO configuration name is bad - NuttX configs should probably all be "px4" -# NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: $(NUTTX_ARCHIVES) @@ -146,16 +144,22 @@ endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh -$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) - @echo %% Configuring NuttX for $(board) +$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && ln -sf $(PX4_BASE)/nuttx-configs/* .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) - @echo %% Exporting NuttX for $(board) + @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ +$(NUTTX_SRC): + @$(ECHO) "" + @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." + @$(ECHO) "" + # # Cleanup targets. 'clean' should remove all built products and force # a complete re-compilation, 'distclean' should remove everything @@ -170,46 +174,47 @@ clean: distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && find . -type l -depth 1 -delete) # # Print some help text # .PHONY: help help: - @echo "" - @echo " PX4 firmware builder" - @echo " ====================" - @echo "" - @echo " Available targets:" - @echo " ------------------" - @echo "" - @echo " archives" - @echo " Build the NuttX RTOS archives that are used by the firmware build." - @echo "" - @echo " all" - @echo " Build all firmware configs: $(CONFIGS)" - @echo " A limited set of configs can be built with CONFIGS=" - @echo "" + @$(ECHO) "" + @$(ECHO) " PX4 firmware builder" + @$(ECHO) " ====================" + @$(ECHO) "" + @$(ECHO) " Available targets:" + @$(ECHO) " ------------------" + @$(ECHO) "" + @$(ECHO) " archives" + @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." + @$(ECHO) "" + @$(ECHO) " all" + @$(ECHO) " Build all firmware configs: $(CONFIGS)" + @$(ECHO) " A limited set of configs can be built with CONFIGS=" + @$(ECHO) "" @for config in $(CONFIGS); do \ echo " $$config"; \ echo " Build just the $$config firmware configuration."; \ echo ""; \ done - @echo " clean" - @echo " Remove all firmware build pieces." - @echo "" - @echo " distclean" - @echo " Remove all compilation products, including NuttX RTOS archives." - @echo "" - @echo " upload" - @echo " When exactly one config is being built, add this target to upload the" - @echo " firmware to the board when the build is complete. Not supported for" - @echo " all configurations." - @echo "" - @echo " Common options:" - @echo " ---------------" - @echo "" - @echo " V=1" - @echo " If V is set, more verbose output is printed during the build. This can" - @echo " help when diagnosing issues with the build or toolchain." - @echo "" + @$(ECHO) " clean" + @$(ECHO) " Remove all firmware build pieces." + @$(ECHO) "" + @$(ECHO) " distclean" + @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." + @$(ECHO) "" + @$(ECHO) " upload" + @$(ECHO) " When exactly one config is being built, add this target to upload the" + @$(ECHO) " firmware to the board when the build is complete. Not supported for" + @$(ECHO) " all configurations." + @$(ECHO) "" + @$(ECHO) " Common options:" + @$(ECHO) " ---------------" + @$(ECHO) "" + @$(ECHO) " V=1" + @$(ECHO) " If V is set, more verbose output is printed during the build. This can" + @$(ECHO) " help when diagnosing issues with the build or toolchain." + @$(ECHO) "" diff --git a/makefiles/board_px4fmu-v2.mk b/makefiles/board_px4fmu-v2.mk new file mode 100644 index 000000000..4b3b7e585 --- /dev/null +++ b/makefiles/board_px4fmu-v2.mk @@ -0,0 +1,10 @@ +# +# Board-specific definitions for the PX4FMUv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4fmuv2.mk b/makefiles/board_px4fmuv2.mk deleted file mode 100644 index 4b3b7e585..000000000 --- a/makefiles/board_px4fmuv2.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Board-specific definitions for the PX4FMUv2 -# - -# -# Configure the toolchain -# -CONFIG_ARCH = CORTEXM4F - -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4io-v2.mk b/makefiles/board_px4io-v2.mk new file mode 100644 index 000000000..ee6b6125e --- /dev/null +++ b/makefiles/board_px4io-v2.mk @@ -0,0 +1,10 @@ +# +# Board-specific definitions for the PX4IOv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM3 + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4iov2.mk b/makefiles/board_px4iov2.mk deleted file mode 100644 index ee6b6125e..000000000 --- a/makefiles/board_px4iov2.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Board-specific definitions for the PX4IOv2 -# - -# -# Configure the toolchain -# -CONFIG_ARCH = CORTEXM3 - -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk new file mode 100644 index 000000000..c4499048c --- /dev/null +++ b/makefiles/config_px4fmu-v2_default.mk @@ -0,0 +1,122 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/rgbled +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +#MODULES += drivers/mpu6000 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott_telemetry +MODULES += drivers/blinkm +MODULES += modules/sensors + +# +# System commands +# +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +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 + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard + +# +# Estimation modules (EKF / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/position_estimator_mc +MODULES += modules/position_estimator +MODULES += modules/att_pos_estimator_ekf + +# +# Vehicle Control +# +MODULES += modules/fixedwing_backside +MODULES += modules/fixedwing_att_control +MODULES += modules/fixedwing_pos_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control + +# +# Logging +# +MODULES += modules/sdlog + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/mathlib +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += modules/mathlib/CMSIS + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +#MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk new file mode 100644 index 000000000..9b3013a5b --- /dev/null +++ b/makefiles/config_px4fmu-v2_test.mk @@ -0,0 +1,40 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/boards/px4fmuv2 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmuv2_default.mk b/makefiles/config_px4fmuv2_default.mk deleted file mode 100644 index c4499048c..000000000 --- a/makefiles/config_px4fmuv2_default.mk +++ /dev/null @@ -1,122 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/px4fmu -MODULES += drivers/px4io -MODULES += drivers/boards/px4fmuv2 -MODULES += drivers/rgbled -MODULES += drivers/lsm303d -MODULES += drivers/l3gd20 -#MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/mb12xx -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/hott_telemetry -MODULES += drivers/blinkm -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -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 - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard - -# -# Estimation modules (EKF / other filters) -# -MODULES += modules/attitude_estimator_ekf -MODULES += modules/position_estimator_mc -MODULES += modules/position_estimator -MODULES += modules/att_pos_estimator_ekf - -# -# Vehicle Control -# -MODULES += modules/fixedwing_backside -MODULES += modules/fixedwing_att_control -MODULES += modules/fixedwing_pos_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control - -# -# Logging -# -MODULES += modules/sdlog - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/mathlib -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += modules/mathlib/CMSIS - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmuv2_test.mk b/makefiles/config_px4fmuv2_test.mk deleted file mode 100644 index 0bd6c18e5..000000000 --- a/makefiles/config_px4fmuv2_test.mk +++ /dev/null @@ -1,45 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/px4io -MODULES += drivers/boards/px4fmuv2 -MODULES += systemcmds/perf -MODULES += systemcmds/reboot - -# needed because things use it for logging -MODULES += modules/mavlink - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/uORB - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk new file mode 100644 index 000000000..f9f35d629 --- /dev/null +++ b/makefiles/config_px4io-v2_default.mk @@ -0,0 +1,10 @@ +# +# Makefile for the px4iov2_default configuration +# + +# +# Board support modules +# +MODULES += drivers/stm32 +MODULES += drivers/boards/px4iov2 +MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/makefiles/config_px4iov2_default.mk b/makefiles/config_px4iov2_default.mk deleted file mode 100644 index f9f35d629..000000000 --- a/makefiles/config_px4iov2_default.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Makefile for the px4iov2_default configuration -# - -# -# Board support modules -# -MODULES += drivers/stm32 -MODULES += drivers/boards/px4iov2 -MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 92461fafb..a0e880a0d 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -45,7 +45,6 @@ export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ -export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 4b01b447d..c55e3f188 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -27,7 +27,7 @@ all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) -upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) +upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # diff --git a/nuttx-configs/px4fmu-v2/Kconfig b/nuttx-configs/px4fmu-v2/Kconfig new file mode 100644 index 000000000..069b25f9e --- /dev/null +++ b/nuttx-configs/px4fmu-v2/Kconfig @@ -0,0 +1,7 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMU_V2 +endif diff --git a/nuttx-configs/px4fmu-v2/common/Make.defs b/nuttx-configs/px4fmu-v2/common/Make.defs new file mode 100644 index 000000000..be387dce1 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/common/Make.defs @@ -0,0 +1,184 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMUV2 +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v2/common/ld.script b/nuttx-configs/px4fmu-v2/common/ld.script new file mode 100644 index 000000000..1be39fb87 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/common/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h new file mode 100755 index 000000000..a6cdfb4d2 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -0,0 +1,364 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* High-resolution timer + */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ +#define GPIO_USART1_TX 0 /* USART1 is RX-only */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + * + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * I2C busses + */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs new file mode 100644 index 000000000..00257d546 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu-v2/common/Make.defs diff --git a/nuttx-configs/px4fmu-v2/nsh/appconfig b/nuttx-configs/px4fmu-v2/nsh/appconfig new file mode 100644 index 000000000..0e18aa8ef --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/appconfig @@ -0,0 +1,52 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig new file mode 100644 index 000000000..efbb883a5 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -0,0 +1,1022 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SDIO_DMA=y +# CONFIG_SDIO_DMAPRIO is not set +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +# CONFIG_STM32_I2C3 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +CONFIG_STM32_USART6=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART4_RS485 is not set +CONFIG_UART4_RXDMA=y +# CONFIG_UART5_RXDMA is not set +# CONFIG_USART6_RS485 is not set +# CONFIG_USART6_RXDMA is not set +# CONFIG_USART7_RXDMA is not set +CONFIG_USART8_RXDMA=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V2=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4fmu-v2" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +# CONFIG_DEV_CONSOLE is not set +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=8 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +# CONFIG_MTD_PARTITION is not set +# CONFIG_MTD_BYTE_WRITE is not set + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +# CONFIG_UART7_SERIAL_CONSOLE is not set +# CONFIG_UART8_SERIAL_CONSOLE is not set +CONFIG_NO_SERIAL_CONSOLE=y + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=16 +CONFIG_USART1_TXBUFSIZE=16 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=128 +CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=16 +CONFIG_USART6_TXBUFSIZE=16 +CONFIG_USART6_BAUD=115200 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=128 +CONFIG_UART7_TXBUFSIZE=128 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=128 +CONFIG_UART8_TXBUFSIZE=128 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG=y +CONFIG_SYSLOG_CHAR=y +CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +CONFIG_GRAN_SINGLE=y +CONFIG_GRAN_INTR=y + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig.prev b/nuttx-configs/px4fmu-v2/nsh/defconfig.prev new file mode 100755 index 000000000..42910ce0a --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig.prev @@ -0,0 +1,1067 @@ +############################################################################ +# configs/px4fmu/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization +# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F427V=y +CONFIG_ARCH_BOARD="px4fmu-v2" +CONFIG_ARCH_BOARD_PX4FMU_V2=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DRAM_SIZE=0x00040000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_STM32_STM32F427=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=n +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y + +# +# On-chip CCM SRAM configuration +# +# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need +# to do this if DMA is enabled to prevent non-DMA-able CCM memory from +# being a part of the stack. +# +CONFIG_STM32_CCMEXCLUDE=y + +# +# On-board FSMC SRAM configuration +# +# CONFIG_STM32_FSMC - Required. See below +# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) +# +# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the +# FSMC (as opposed to an LCD or FLASH). +# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space +# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space +# +#CONFIG_STM32_FSMC_SRAM=n +#CONFIG_HEAP2_BASE=0x64000000 +#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) + +# +# Individual subsystems can be enabled: +# +# This set is exhaustive for PX4FMU and should be safe to cut and +# paste into any other config. +# +# AHB1: +CONFIG_STM32_CRC=n +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_ETHMAC=n +CONFIG_STM32_OTGHS=n +# AHB2: +CONFIG_STM32_DCMI=n +CONFIG_STM32_CRYP=n +CONFIG_STM32_HASH=n +CONFIG_STM32_RNG=n +CONFIG_STM32_OTGFS=y +# AHB3: +CONFIG_STM32_FSMC=n +# APB1: +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_TIM12=n +CONFIG_STM32_TIM13=n +CONFIG_STM32_TIM14=n +CONFIG_STM32_WWDG=y +CONFIG_STM32_IWDG=n +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI3=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART5=n +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=n +CONFIG_STM32_CAN1=n +CONFIG_STM32_CAN2=n +CONFIG_STM32_DAC=n +CONFIG_STM32_PWR=y +# APB2: +CONFIG_STM32_TIM1=y +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +# Mostly owned by the px4io driver, but uploader needs this +CONFIG_STM32_USART6=y +# We use our own driver, but leave this on. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +CONFIG_STM32_ADC3=n +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y + +# +# Enable single wire support. If this is not defined, then this mode cannot +# be enabled. +# +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# We want the flash prefetch on for max performance. +# +STM32_FLASH_PREFETCH=y + +# +# STM32F40xxx specific serial device driver settings +# +# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, +# tcflush, etc.). If this is not defined, then the terminal settings (baud, +# parity, etc.) are not configurable at runtime; serial streams cannot be +# flushed, etc. +# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port +# immediately after creating the /dev/console device. This is required +# if the console serial port has RX DMA enabled. +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_CONSOLE_REINIT=n +CONFIG_STANDARD_SERIAL=y + +CONFIG_UART8_SERIAL_CONSOLE=y + +#Mavlink messages can be bigger than 128 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=256 +CONFIG_USART3_TXBUFSIZE=256 +CONFIG_UART4_TXBUFSIZE=256 +CONFIG_USART6_TXBUFSIZE=128 +CONFIG_UART7_TXBUFSIZE=256 +CONFIG_UART8_TXBUFSIZE=256 + +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=256 +CONFIG_USART3_RXBUFSIZE=256 +CONFIG_UART4_RXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=256 +CONFIG_UART7_RXBUFSIZE=256 +CONFIG_UART8_RXBUFSIZE=256 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 +CONFIG_UART4_BAUD=115200 +CONFIG_USART6_BAUD=115200 +CONFIG_UART7_BAUD=115200 +CONFIG_UART8_BAUD=57600 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 +CONFIG_UART4_BITS=8 +CONFIG_USART6_BITS=8 +CONFIG_UART7_BITS=8 +CONFIG_UART8_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 +CONFIG_UART4_PARITY=0 +CONFIG_USART6_PARITY=0 +CONFIG_UART7_PARITY=0 +CONFIG_UART8_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 +CONFIG_UART4_2STOP=0 +CONFIG_USART6_2STOP=0 +CONFIG_UART7_2STOP=0 +CONFIG_UART8_2STOP=0 + +CONFIG_USART1_RXDMA=y +CONFIG_USART2_RXDMA=y +CONFIG_USART3_RXDMA=n +CONFIG_UART4_RXDMA=n +CONFIG_USART6_RXDMA=y +CONFIG_UART7_RXDMA=n +CONFIG_UART8_RXDMA=n + +# +# STM32F40xxx specific SPI device driver settings +# +CONFIG_SPI_EXCHANGE=y +# DMA needs more work, not implemented on STM32F4x yet +#CONFIG_STM32_SPI_DMA=y + +# +# STM32F40xxx specific CAN device driver settings +# +# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or +# CONFIG_STM32_CAN2 must also be defined) +# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default +# Standard 11-bit IDs. +# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. +# Default: 8 +# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. +# Default: 4 +# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback +# mode for testing. The STM32 CAN driver does support loopback mode. +# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. +# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. +# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 +# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 +# +CONFIG_CAN=n +CONFIG_CAN_EXTID=n +#CONFIG_CAN_FIFOSIZE +#CONFIG_CAN_NPENDINGRTR +CONFIG_CAN_LOOPBACK=n +CONFIG_CAN1_BAUD=700000 +CONFIG_CAN2_BAUD=700000 + + +# XXX remove after integration testing +# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using +# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update +CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 +# Constant overhead for generating I2C start / stop conditions +CONFIG_STM32_I2CTIMEOUS_START_STOP=700 +# XXX this is bad and we want it gone +CONFIG_I2C_WRITEREAD=y + +# +# I2C configuration +# +CONFIG_I2C=y +CONFIG_I2C_POLLED=n +CONFIG_I2C_TRANSFER=y +CONFIG_I2C_TRACE=n +CONFIG_I2C_RESET=y +# XXX fixed per-transaction timeout +CONFIG_STM32_I2CTIMEOMS=10 + +# +# MTD support +# +CONFIG_MTD=y + +# XXX re-enable after integration testing + +# +# I2C configuration +# +#CONFIG_I2C=y +#CONFIG_I2C_POLLED=y +#CONFIG_I2C_TRANSFER=y +#CONFIG_I2C_TRACE=n +#CONFIG_I2C_RESET=y + +# Dynamic timeout +#CONFIG_STM32_I2C_DYNTIMEO=y +#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 +#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 + +# Fixed per-transaction timeout +#CONFIG_STM32_I2CTIMEOSEC=0 +#CONFIG_STM32_I2CTIMEOMS=10 + + + + + + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=y + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 192 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single +# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined +# then an additional, lower-priority work queue will also be created. This +# lower priority work queue is better suited for more extended processing +# (such as file system clean-up operations) +# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority +# worker thread. Default: 50 +# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread +# checks for work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower +# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="nsh_main" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=y +CONFIG_DEBUG_VERBOSE=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=2 +CONFIG_ARCH_LOWPUTC=y +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=y +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=y +CONFIG_SCHED_ATEXIT=n + +# +# System Logging +# +# CONFIG_SYSLOG - Enables the System Logging feature. +# CONFIG_RAMLOG - Enables the RAM logging feature +# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. +# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all +# console output will be re-directed to a circular buffer in RAM. This +# is useful, for example, if the only console is a Telnet console. Then +# in that case, console output from non-Telnet threads will go to the +# circular buffer and can be viewed using the NSH 'dmesg' command. +# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging +# interface. If this feature is enabled (along with CONFIG_SYSLOG), +# then all debug output (only) will be re-directed to the circular +# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' +# command. +# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting +# for this driver on poll(). Default: 4 +# +# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the +# following may also be provided: +# +# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 +# + +CONFIG_SYSLOG=n +CONFIG_RAMLOG=n +CONFIG_RAMLOG_CONSOLE=n +CONFIG_RAMLOG_SYSLOG=n +#CONFIG_RAMLOG_NPOLLWAITERS +#CONFIG_RAMLOG_CONSOLE_BUFSIZE + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=n +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=n +CONFIG_DISABLE_MOUNTPOINT=n +CONFIG_DISABLE_ENVIRON=n +CONFIG_DISABLE_POLL=n + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") +# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: +# 5.1234567 +# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) +# +CONFIG_NOPRINTF_FIELDWIDTH=n +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_HAVE_LONG_LONG=y + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=y +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=8 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Filesystem configuration +# +# CONFIG_FS_FAT - Enable FAT filesystem support +# CONFIG_FAT_SECTORSIZE - Max supported sector size +# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 +# file name support. +# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims +# patents on FAT long file name technology. Please read the +# disclaimer in the top-level COPYING file and only enable this +# feature if you understand these issues. +# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the +# default, maximum long file name is 255 bytes. This can eat up +# a lot of memory (especially stack space). If you are willing +# to live with some non-standard, short long file names, then +# define this value. A good choice would be the same value as +# selected for CONFIG_NAME_MAX which will limit the visibility +# of longer file names anyway. +# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. +# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. +# This must have one of the values of 0xff or 0x00. +# Default: 0xff. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. +# Default: 255. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# Default: 32. +# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean +# packing files together toward the end of the file or, if file are +# deleted at the end of the file, clean up can simply mean erasing +# the end of FLASH memory so that it can be re-used again. However, +# doing this can also harm the life of the FLASH part because it can +# mean that the tail end of the FLASH is re-used too often. This +# threshold determines if/when it is worth erased the tail end of FLASH +# and making it available for re-use (and possible over-wear). +# Default: 8192. +# CONFIG_FS_ROMFS - Enable ROMFS filesystem support +# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this +# option will enable a limited form of memory mapping that is +# implemented by copying whole files into memory. +# +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_FS_ROMFS=y +CONFIG_FS_BINFS=y + +# +# SPI-based MMC/SD driver +# +# CONFIG_MMCSD_NSLOTS +# Number of MMC/SD slots supported by the driver +# CONFIG_MMCSD_READONLY +# Provide read-only access (default is read/write) +# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. +# Default is 20MHz, current setting 24 MHz +# +#CONFIG_MMCSD=n +# XXX need to rejig this for SDIO +#CONFIG_MMCSD_SPI=y +#CONFIG_MMCSD_NSLOTS=1 +#CONFIG_MMCSD_READONLY=n +#CONFIG_MMCSD_SPICLOCK=24000000 + +# +# Maintain legacy build behavior (revisit) +# + +CONFIG_MMCSD=y +#CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y + +# +# SPI-based MMC/SD driver +# +#CONFIG_MMCSD_NSLOTS=1 +#CONFIG_MMCSD_READONLY=n +#CONFIG_MMCSD_SPICLOCK=12500000 + +# +# STM32 SDIO-based MMC/SD driver +# +CONFIG_SDIO_DMA=y +#CONFIG_SDIO_PRI=128 +#CONFIG_SDIO_DMAPRIO +#CONFIG_SDIO_WIDTH_D1_ONLY +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_MMCSUPPORT=n +CONFIG_MMCSD_HAVECARDDETECT=n + +# +# Block driver buffering +# +# CONFIG_FS_READAHEAD +# Enable read-ahead buffering +# CONFIG_FS_WRITEBUFFER +# Enable write buffering +# +CONFIG_FS_READAHEAD=n +CONFIG_FS_WRITEBUFFER=n + +# +# RTC Configuration +# +# CONFIG_RTC - Enables general support for a hardware RTC. Specific +# architectures may require other specific settings. +# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple +# battery backed counter that keeps the time when power is down, and (2) +# A full date / time RTC the provides the date and time information, often +# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this +# second kind of RTC. In this case, the RTC is used to "seed" the normal +# NuttX timer and the NuttX system timer provides for higher resoution +# time. +# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, +# battery backed counter is used. There are two different implementations +# of such simple counters based on the time resolution of the counter: +# The typical RTC keeps time to resolution of 1 second, usually +# supporting a 32-bit time_t value. In this case, the RTC is used to +# "seed" the normal NuttX timer and the NuttX timer provides for higher +# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, +# then the RTC provides higher resolution time and completely replaces the +# system timer for purpose of date and time. +# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency +# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is +# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. +# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an +# alarm. A callback function will be executed when the alarm goes off +# +CONFIG_RTC=n +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HIRES=n +CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_ALARM=n + +# +# USB Device Configuration +# +# CONFIG_USBDEV +# Enables USB device support +# CONFIG_USBDEV_ISOCHRONOUS +# Build in extra support for isochronous endpoints +# CONFIG_USBDEV_DUALSPEED +# Hardware handles high and full speed operation (USB 2.0) +# CONFIG_USBDEV_SELFPOWERED +# Will cause USB features to indicate that the device is +# self-powered +# CONFIG_USBDEV_MAXPOWER +# Maximum power consumption in mA +# CONFIG_USBDEV_TRACE +# Enables USB tracing for debug +# CONFIG_USBDEV_TRACE_NRECORDS +# Number of trace entries to remember +# +CONFIG_USBDEV=y +CONFIG_USBDEV_ISOCHRONOUS=n +CONFIG_USBDEV_DUALSPEED=n +CONFIG_USBDEV_SELFPOWERED=y +CONFIG_USBDEV_REMOTEWAKEUP=n +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USBDEV_TRACE=n +CONFIG_USBDEV_TRACE_NRECORDS=512 + +# +# USB serial device class driver (Standard CDC ACM class) +# +# CONFIG_CDCACM +# Enable compilation of the USB serial driver +# CONFIG_CDCACM_CONSOLE +# Configures the CDC/ACM serial port as the console device. +# CONFIG_CDCACM_EP0MAXPACKET +# Endpoint 0 max packet size. Default 64 +# CONFIG_CDCACM_EPINTIN +# The logical 7-bit address of a hardware endpoint that supports +# interrupt IN operation. Default 2. +# CONFIG_CDCACM_EPINTIN_FSSIZE +# Max package size for the interrupt IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPINTIN_HSSIZE +# Max package size for the interrupt IN endpoint if high speed mode. +# Default 64 +# CONFIG_CDCACM_EPBULKOUT +# The logical 7-bit address of a hardware endpoint that supports +# bulk OUT operation. Default 4. +# CONFIG_CDCACM_EPBULKOUT_FSSIZE +# Max package size for the bulk OUT endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKOUT_HSSIZE +# Max package size for the bulk OUT endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_EPBULKIN +# The logical 7-bit address of a hardware endpoint that supports +# bulk IN operation. Default 3. +# CONFIG_CDCACM_EPBULKIN_FSSIZE +# Max package size for the bulk IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKIN_HSSIZE +# Max package size for the bulk IN endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS +# The number of write/read requests that can be in flight. +# Default 256. +# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR +# The vendor ID code/string. Default 0x0525 and "NuttX" +# 0x0525 is the Netchip vendor and should not be used in any +# products. This default VID was selected for compatibility with +# the Linux CDC ACM default VID. +# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR +# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" +# 0xa4a7 was selected for compatibility with the Linux CDC ACM +# default PID. +# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE +# Size of the serial receive/transmit buffers. Default 256. +# +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=n +#CONFIG_CDCACM_EP0MAXPACKET +CONFIG_CDCACM_EPINTIN=1 +#CONFIG_CDCACM_EPINTIN_FSSIZE +#CONFIG_CDCACM_EPINTIN_HSSIZE +CONFIG_CDCACM_EPBULKOUT=3 +#CONFIG_CDCACM_EPBULKOUT_FSSIZE +#CONFIG_CDCACM_EPBULKOUT_HSSIZE +CONFIG_CDCACM_EPBULKIN=2 +#CONFIG_CDCACM_EPBULKIN_FSSIZE +#CONFIG_CDCACM_EPBULKIN_HSSIZE +#CONFIG_CDCACM_NWRREQS +#CONFIG_CDCACM_NRDREQS +CONFIG_CDCACM_VENDORID=0x26AC +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" +#CONFIG_CDCACM_RXBUFSIZE +#CONFIG_CDCACM_TXBUFSIZE + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_BUILTIN=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAX_ARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_DISABLESCRIPT=n +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_USBCONSOLE=n +#CONFIG_NSH_USBCONDEV="/dev/ttyACM0" +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_IOBUFFER_SIZE=512 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=y +CONFIG_NSH_IPADDR=0x0a000002 +CONFIG_NSH_DRIPADDR=0x0a000001 +CONFIG_NSH_NETMASK=0xffffff00 +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +#CONFIG_NSH_MMCSDSPIPORTNO=3 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 + + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3240G-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +# Idle thread needs 4096 bytes +# default 1 KB is not enough +# 4096 bytes +CONFIG_IDLETHREAD_STACKSIZE=6000 +# USERMAIN stack size probably needs to be around 4096 bytes +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= + +# enable bindir +CONFIG_APPS_BINDIR=y diff --git a/nuttx-configs/px4fmu-v2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh new file mode 100755 index 000000000..265520997 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile new file mode 100644 index 000000000..d4276f7fc --- /dev/null +++ b/nuttx-configs/px4fmu-v2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4io-v2/README.txt b/nuttx-configs/px4io-v2/README.txt new file mode 100755 index 000000000..9b1615f42 --- /dev/null +++ b/nuttx-configs/px4io-v2/README.txt @@ -0,0 +1,806 @@ +README +====== + +This README discusses issues unique to NuttX configurations for the +STMicro STM3210E-EVAL development board. + +Contents +======== + + - Development Environment + - GNU Toolchain Options + - IDEs + - NuttX buildroot Toolchain + - DFU and JTAG + - OpenOCD + - LEDs + - Temperature Sensor + - RTC + - STM3210E-EVAL-specific Configuration Options + - Configurations + +Development Environment +======================= + + Either Linux or Cygwin on Windows can be used for the development environment. + The source has been built only using the GNU toolchain (see below). Other + toolchains will likely cause problems. Testing was performed using the Cygwin + environment because the Raisonance R-Link emulatator and some RIDE7 development tools + were used and those tools works only under Windows. + +GNU Toolchain Options +===================== + + The NuttX make system has been modified to support the following different + toolchain options. + + 1. The CodeSourcery GNU toolchain, + 2. The devkitARM GNU toolchain, + 3. Raisonance GNU toolchain, or + 4. The NuttX buildroot Toolchain (see below). + + All testing has been conducted using the NuttX buildroot toolchain. However, + the make system is setup to default to use the devkitARM toolchain. To use + the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to + add one of the following configuration options to your .config (or defconfig) + file: + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux + CONFIG_STM32_DEVKITARM=y : devkitARM under Windows + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) + + If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify + the PATH in the setenv.h file if your make cannot find the tools. + + NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are + Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot + toolchains are Cygwin and/or Linux native toolchains. There are several limitations + to using a Windows based toolchain in a Cygwin environment. The three biggest are: + + 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are + performed automatically in the Cygwin makefiles using the 'cygpath' utility + but you might easily find some new path problems. If so, check out 'cygpath -w' + + 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links + are used in Nuttx (e.g., include/arch). The make system works around these + problems for the Windows tools by copying directories instead of linking them. + But this can also cause some confusion for you: For example, you may edit + a file in a "linked" directory and find that your changes had no effect. + That is because you are building the copy of the file in the "fake" symbolic + directory. If you use a Windows toolchain, you should get in the habit of + making like this: + + make clean_context all + + An alias in your .bashrc file might make that less painful. + + 3. Dependencies are not made when using Windows versions of the GCC. This is + because the dependencies are generated using Windows pathes which do not + work with the Cygwin make. + + Support has been added for making dependencies with the windows-native toolchains. + That support can be enabled by modifying your Make.defs file as follows: + + - MKDEP = $(TOPDIR)/tools/mknulldeps.sh + + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" + + If you have problems with the dependency build (for example, if you are not + building on C:), then you may need to modify tools/mkdeps.sh + + NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization + level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with + -Os. + + NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that + the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM + path or will get the wrong version of make. + +IDEs +==== + + NuttX is built using command-line make. It can be used with an IDE, but some + effort will be required to create the project (There is a simple RIDE project + in the RIDE subdirectory). + + Makefile Build + -------------- + Under Eclipse, it is pretty easy to set up an "empty makefile project" and + simply use the NuttX makefile to build the system. That is almost for free + under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty + makefile project in order to work with Windows (Google for "Eclipse Cygwin" - + there is a lot of help on the internet). + + Native Build + ------------ + Here are a few tips before you start that effort: + + 1) Select the toolchain that you will be using in your .config file + 2) Start the NuttX build at least one time from the Cygwin command line + before trying to create your project. This is necessary to create + certain auto-generated files and directories that will be needed. + 3) Set up include pathes: You will need include/, arch/arm/src/stm32, + arch/arm/src/common, arch/arm/src/armv7-m, and sched/. + 4) All assembly files need to have the definition option -D __ASSEMBLY__ + on the command line. + + Startup files will probably cause you some headaches. The NuttX startup file + is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX + one time from the Cygwin command line in order to obtain the pre-built + startup object needed by RIDE. + +NuttX buildroot Toolchain +========================= + + A GNU GCC-based toolchain is assumed. The files */setenv.sh should + be modified to point to the correct path to the Cortex-M3 GCC toolchain (if + different from the default in your PATH variable). + + If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX + SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). + This GNU toolchain builds and executes in the Linux or Cygwin environment. + + 1. You must have already configured Nuttx in /nuttx. + + cd tools + ./configure.sh stm3210e-eval/ + + 2. Download the latest buildroot package into + + 3. unpack the buildroot tarball. The resulting directory may + have versioning information on it like buildroot-x.y.z. If so, + rename /buildroot-x.y.z to /buildroot. + + 4. cd /buildroot + + 5. cp configs/cortexm3-defconfig-4.3.3 .config + + 6. make oldconfig + + 7. make + + 8. Edit setenv.h, if necessary, so that the PATH variable includes + the path to the newly built binaries. + + See the file configs/README.txt in the buildroot source tree. That has more + detailed PLUS some special instructions that you will need to follow if you are + building a Cortex-M3 toolchain for Cygwin under Windows. + +DFU and JTAG +============ + + Enbling Support for the DFU Bootloader + -------------------------------------- + The linker files in these projects can be configured to indicate that you + will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) + loader or via some JTAG emulator. You can specify the DFU bootloader by + adding the following line: + + CONFIG_STM32_DFU=y + + to your .config file. Most of the configurations in this directory are set + up to use the DFU loader. + + If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning + of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed + to make space for the DFU loader and 0x08003000 is where the DFU loader expects + to find new applications at boot time. If you need to change that origin for some + other bootloader, you will need to edit the file(s) ld.script.dfu for each + configuration. + + The DFU SE PC-based software is available from the STMicro website, + http://www.st.com. General usage instructions: + + 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU + file (nuttx.dfu)... see below for details. + 2. Connect the STM3210E-EVAL board to your computer using a USB + cable. + 3. Start the DFU loader on the STM3210E-EVAL board. You do this by + resetting the board while holding the "Key" button. Windows should + recognize that the DFU loader has been installed. + 3. Run the DFU SE program to load nuttx.dfu into FLASH. + + What if the DFU loader is not in FLASH? The loader code is available + inside of the Demo dirctory of the USBLib ZIP file that can be downloaded + from the STMicro Website. You can build it using RIDE (or other toolchains); + you will need a JTAG emulator to burn it into FLASH the first time. + + In order to use STMicro's built-in DFU loader, you will have to get + the NuttX binary into a special format with a .dfu extension. The + DFU SE PC_based software installation includes a file "DFU File Manager" + conversion program that a file in Intel Hex format to the special DFU + format. When you successfully build NuttX, you will find a file called + nutt.ihx in the top-level directory. That is the file that you should + provide to the DFU File Manager. You will need to rename it to nuttx.hex + in order to find it with the DFU File Manager. You will end up with + a file called nuttx.dfu that you can use with the STMicro DFU SE program. + + Enabling JTAG + ------------- + If you are not using the DFU, then you will probably also need to enable + JTAG support. By default, all JTAG support is disabled but there NuttX + configuration options to enable JTAG in various different ways. + + These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO + MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the + Cortex debug port. The default state in this port is for all JTAG support + to be disable. + + CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full + SWJ (JTAG-DP + SW-DP) + + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable + full SWJ (JTAG-DP + SW-DP) but without JNTRST. + + CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP + disabled and SW-DP enabled + + The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 + which disable JTAG-DP and SW-DP. + +OpenOCD +======= + +I have also used OpenOCD with the STM3210E-EVAL. In this case, I used +the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh +for more information. Using the script: + +1) Start the OpenOCD GDB server + + cd + configs/stm3210e-eval/tools/oocd.sh $PWD + +2) Load Nuttx + + cd + arm-none-eabi-gdb nuttx + gdb> target remote localhost:3333 + gdb> mon reset + gdb> mon halt + gdb> load nuttx + +3) Running NuttX + + gdb> mon reset + gdb> c + +LEDs +==== + +The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the +board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is +defined. In that case, the usage by the board port is defined in +include/board.h and src/up_leds.c. The LEDs are used to encode OS-related +events as follows: + + SYMBOL Meaning LED1* LED2 LED3 LED4 + ---------------- ----------------------- ----- ----- ----- ----- + LED_STARTED NuttX has been started ON OFF OFF OFF + LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF + LED_IRQSENABLED Interrupts enabled ON ON OFF OFF + LED_STACKCREATED Idle stack created OFF OFF ON OFF + LED_INIRQ In an interrupt** ON N/C N/C OFF + LED_SIGNAL In a signal handler*** N/C ON N/C OFF + LED_ASSERTION An assertion failed ON ON N/C OFF + LED_PANIC The system has crashed N/C N/C N/C ON + LED_IDLE STM32 is is sleep mode (Optional, not used) + + * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot + and these LEDs will give you some indication of where the failure was + ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + is because of timer interupts that result in the LED being illuminated + on a small proportion of the time. +*** LED2 may also flicker normally if signals are processed. + +Temperature Sensor +================== + +Support for the on-board LM-75 temperature sensor is available. This supported +has been verified, but has not been included in any of the available the +configurations. To set up the temperature sensor, add the following to the +NuttX configuration file + + CONFIG_I2C=y + CONFIG_I2C_LM75=y + +Then you can implement logic like the following to use the temperature sensor: + + #include + #include + + ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ + fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ + ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ + bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ + +More complex temperature sensor operations are also available. See the IOCTAL +commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions +of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the +arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). + +RTC +=== + + The STM32 RTC may configured using the following settings. + + CONFIG_RTC - Enables general support for a hardware RTC. Specific + architectures may require other specific settings. + CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 + second, usually supporting a 32-bit time_t value. In this case, + the RTC is used to "seed" the normal NuttX timer and the + NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES + is enabled in the NuttX configuration, then the RTC provides higher + resolution time and completely replaces the system timer for purpose of + date and time. + CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the + frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES + is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. + CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. + A callback function will be executed when the alarm goes off + + In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts + are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. + A BKP register is incremented on each overflow interrupt creating, effectively, + a 48-bit RTC counter. + + In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled + (because the next overflow is not expected until the year 2106. + + WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The + overflow interrupt may be lost even if the STM32 is powered down only momentarily. + Therefore hi-res solution is only useful in systems where the power is always on. + +STM3210E-EVAL-specific Configuration Options +============================================ + + CONFIG_ARCH - Identifies the arch/ subdirectory. This should + be set to: + + CONFIG_ARCH=arm + + CONFIG_ARCH_family - For use in C code: + + CONFIG_ARCH_ARM=y + + CONFIG_ARCH_architecture - For use in C code: + + CONFIG_ARCH_CORTEXM3=y + + CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory + + CONFIG_ARCH_CHIP=stm32 + + CONFIG_ARCH_CHIP_name - For use in C code to identify the exact + chip: + + CONFIG_ARCH_CHIP_STM32F103ZET6 + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock + configuration features. + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n + + CONFIG_ARCH_BOARD - Identifies the configs subdirectory and + hence, the board that supports the particular chip or SoC. + + CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) + + CONFIG_ARCH_BOARD_name - For use in C code + + CONFIG_ARCH_BOARD_STM3210E_EVAL=y + + CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation + of delay loops + + CONFIG_ENDIAN_BIG - define if big endian (default is little + endian) + + CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): + + CONFIG_DRAM_SIZE=0x00010000 (64Kb) + + CONFIG_DRAM_START - The start address of installed DRAM + + CONFIG_DRAM_START=0x20000000 + + CONFIG_DRAM_END - Last address+1 of installed RAM + + CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) + + CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization + + CONFIG_ARCH_IRQPRIO=y + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that + have LEDs + + CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt + stack. If defined, this symbol is the size of the interrupt + stack in bytes. If not defined, the user task stacks will be + used during interrupt handling. + + CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. + + CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that + cause a 100 second delay during boot-up. This 100 second delay + serves no purpose other than it allows you to calibratre + CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure + the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until + the delay actually is 100 seconds. + + Individual subsystems can be enabled: + AHB + --- + CONFIG_STM32_DMA1 + CONFIG_STM32_DMA2 + CONFIG_STM32_CRC + CONFIG_STM32_FSMC + CONFIG_STM32_SDIO + + APB1 + ---- + CONFIG_STM32_TIM2 + CONFIG_STM32_TIM3 + CONFIG_STM32_TIM4 + CONFIG_STM32_TIM5 + CONFIG_STM32_TIM6 + CONFIG_STM32_TIM7 + CONFIG_STM32_WWDG + CONFIG_STM32_SPI2 + CONFIG_STM32_SPI4 + CONFIG_STM32_USART2 + CONFIG_STM32_USART3 + CONFIG_STM32_UART4 + CONFIG_STM32_UART5 + CONFIG_STM32_I2C1 + CONFIG_STM32_I2C2 + CONFIG_STM32_USB + CONFIG_STM32_CAN + CONFIG_STM32_BKP + CONFIG_STM32_PWR + CONFIG_STM32_DAC1 + CONFIG_STM32_DAC2 + CONFIG_STM32_USB + + APB2 + ---- + CONFIG_STM32_ADC1 + CONFIG_STM32_ADC2 + CONFIG_STM32_TIM1 + CONFIG_STM32_SPI1 + CONFIG_STM32_TIM8 + CONFIG_STM32_USART1 + CONFIG_STM32_ADC3 + + Timer and I2C devices may need to the following to force power to be applied + unconditionally at power up. (Otherwise, the device is powered when it is + initialized). + + CONFIG_STM32_FORCEPOWER + + Timer devices may be used for different purposes. One special purpose is + to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn + is defined (as above) then the following may also be defined to indicate that + the timer is intended to be used for pulsed output modulation, ADC conversion, + or DAC conversion. Note that ADC/DAC require two definition: Not only do you have + to assign the timer (n) for used by the ADC or DAC, but then you also have to + configure which ADC or DAC (m) it is assigned to. + + CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 + CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 + CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 + CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 + CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 + + For each timer that is enabled for PWM usage, we need the following additional + configuration settings: + + CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} + + NOTE: The STM32 timers are each capable of generating different signals on + each of the four channels with different duty cycles. That capability is + not supported by this driver: Only one output channel per timer. + + Alternate pin mappings (should not be used with the STM3210E-EVAL board): + + CONFIG_STM32_TIM1_FULL_REMAP + CONFIG_STM32_TIM1_PARTIAL_REMAP + CONFIG_STM32_TIM2_FULL_REMAP + CONFIG_STM32_TIM2_PARTIAL_REMAP_1 + CONFIG_STM32_TIM2_PARTIAL_REMAP_2 + CONFIG_STM32_TIM3_FULL_REMAP + CONFIG_STM32_TIM3_PARTIAL_REMAP + CONFIG_STM32_TIM4_REMAP + CONFIG_STM32_USART1_REMAP + CONFIG_STM32_USART2_REMAP + CONFIG_STM32_USART3_FULL_REMAP + CONFIG_STM32_USART3_PARTIAL_REMAP + CONFIG_STM32_SPI1_REMAP + CONFIG_STM32_SPI3_REMAP + CONFIG_STM32_I2C1_REMAP + CONFIG_STM32_CAN1_FULL_REMAP + CONFIG_STM32_CAN1_PARTIAL_REMAP + CONFIG_STM32_CAN2_REMAP + + JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): + CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + but without JNTRST. + CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled + + STM32F103Z specific device driver settings + + CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART + m (m=4,5) for the console and ttys0 (default is the USART1). + CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. + This specific the size of the receive buffer + CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before + being sent. This specific the size of the transmit buffer + CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be + CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. + CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity + CONFIG_U[S]ARTn_2STOP - Two stop bits + + CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI + support. Non-interrupt-driven, poll-waiting is recommended if the + interrupt rate would be to high in the interrupt driven case. + CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. + Cannot be used with CONFIG_STM32_SPI_INTERRUPT. + + CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO + and CONFIG_STM32_DMA2. + CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 + CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. + Default: Medium + CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: + 4-bit transfer mode. + + STM3210E-EVAL CAN Configuration + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default + Standard 11-bit IDs. + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 + CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + + STM3210E-EVAL LCD Hardware Configuration + + CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" + support. Default is this 320x240 "landscape" orientation + (this setting is informative only... not used). + CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" + orientation support. In this orientation, the STM3210E-EVAL's + LCD ribbon cable is at the bottom of the display. Default is + 320x240 "landscape" orientation. + CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse + portrait" orientation support. In this orientation, the + STM3210E-EVAL's LCD ribbon cable is at the top of the display. + Default is 320x240 "landscape" orientation. + CONFIG_LCD_BACKLIGHT - Define to support a backlight. + CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an + adjustable backlight will be provided using timer 1 to generate + various pulse widthes. The granularity of the settings is + determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or + CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight + is provided. + CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears + to be a shift in the returned data. This value fixes the offset. + Default 5. + + The LCD driver dynamically selects the LCD based on the reported LCD + ID value. However, code size can be reduced by suppressing support for + individual LCDs using: + + CONFIG_STM32_AM240320_DISABLE + CONFIG_STM32_SPFD5408B_DISABLE + CONFIG_STM32_R61580_DISABLE + +Configurations +============== + +Each STM3210E-EVAL configuration is maintained in a sudirectory and +can be selected as follow: + + cd tools + ./configure.sh stm3210e-eval/ + cd - + . ./setenv.sh + +Where is one of the following: + + buttons: + -------- + + Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and + button interrupts. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + composite + --------- + + This configuration exercises a composite USB interface consisting + of a CDC/ACM device and a USB mass storage device. This configuration + uses apps/examples/composite. + + nsh and nsh2: + ------------ + Configure the NuttShell (nsh) located at examples/nsh. + + Differences between the two NSH configurations: + + =========== ======================= ================================ + nsh nsh2 + =========== ======================= ================================ + Toolchain: NuttX buildroot for Codesourcery for Windows (1) + Linux or Cygwin (1,2) + ----------- ----------------------- -------------------------------- + Loader: DfuSe DfuSe + ----------- ----------------------- -------------------------------- + Serial Debug output: USART1 Debug output: USART1 + Console: NSH output: USART1 NSH output: USART1 (3) + ----------- ----------------------- -------------------------------- + microSD Yes Yes + Support + ----------- ----------------------- -------------------------------- + FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y + Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) + ----------- ----------------------- -------------------------------- + Support for No Yes + Built-in + Apps + ----------- ----------------------- -------------------------------- + Built-in None apps/examples/nx + Apps apps/examples/nxhello + apps/examples/usbstorage (5) + =========== ======================= ================================ + + (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh + to set up the correct PATH variable for whichever toolchain you + may use. + (2) Since DfuSe is assumed, this configuration may only work under + Cygwin without modification. + (3) When any other device other than /dev/console is used for a user + interface, (1) linefeeds (\n) will not be expanded to carriage return + / linefeeds \r\n). You will need to configure your terminal program + to account for this. And (2) input is not automatically echoed so + you will have to turn local echo on. + (4) Microsoft holds several patents related to the design of + long file names in the FAT file system. Please refer to the + details in the top-level COPYING file. Please do not use FAT + long file name unless you are familiar with these patent issues. + (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), + Caution should be used to assure that the SD drive is not in use when + the USB storage device is configured. Specifically, the SD driver + should be unmounted like: + + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH + ... + nsh> umount /mnd/sdcard # Unmount before connecting USB!!! + nsh> msconn # Connect the USB storage device + ... + nsh> msdis # Disconnect USB storate device + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount + + Failure to do this could result in corruption of the SD card format. + + nx: + --- + An example using the NuttX graphics system (NX). This example + focuses on general window controls, movement, mouse and keyboard + input. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxlines: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing lines on the background in various + orientations. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxtext: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing text on the background while pop-up + windows occur. Text should continue to update normally with + or without the popup windows present. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + NOTE: When I tried building this example with the CodeSourcery + tools, I got a hardfault inside of its libgcc. I haven't + retested since then, but beware if you choose to change the + toolchain. + + ostest: + ------ + This configuration directory, performs a simple OS test using + examples/ostest. By default, this project assumes that you are + using the DFU bootloader. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + RIDE + ---- + This configuration builds a trivial bring-up binary. It is + useful only because it words with the RIDE7 IDE and R-Link debugger. + + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + + usbserial: + --------- + This configuration directory exercises the USB serial class + driver at examples/usbserial. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + USB debug output can be enabled as by changing the following + settings in the configuration file: + + -CONFIG_DEBUG=n + -CONFIG_DEBUG_VERBOSE=n + -CONFIG_DEBUG_USB=n + +CONFIG_DEBUG=y + +CONFIG_DEBUG_VERBOSE=y + +CONFIG_DEBUG_USB=y + + -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n + -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n + +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y + +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y + + By default, the usbserial example uses the Prolific PL2303 + serial/USB converter emulation. The example can be modified + to use the CDC/ACM serial class by making the following changes + to the configuration file: + + -CONFIG_PL2303=y + +CONFIG_PL2303=n + + -CONFIG_CDCACM=n + +CONFIG_CDCACM=y + + The example can also be converted to use the alternative + USB serial example at apps/examples/usbterm by changing the + following: + + -CONFIGURED_APPS += examples/usbserial + +CONFIGURED_APPS += examples/usbterm + + In either the original appconfig file (before configuring) + or in the final apps/.config file (after configuring). + + usbstorage: + ---------- + This configuration directory exercises the USB mass storage + class driver at examples/usbstorage. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + diff --git a/nuttx-configs/px4io-v2/common/Make.defs b/nuttx-configs/px4io-v2/common/Make.defs new file mode 100644 index 000000000..7f782b5b2 --- /dev/null +++ b/nuttx-configs/px4io-v2/common/Make.defs @@ -0,0 +1,175 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# enable precise stack overflow tracking +#INSTRUMENTATIONDEFINES = -finstrument-functions \ +# -ffixed-r10 + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4io-v2/common/ld.script b/nuttx-configs/px4io-v2/common/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx-configs/px4io-v2/common/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v2/common/setenv.sh b/nuttx-configs/px4io-v2/common/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx-configs/px4io-v2/common/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v2/include/README.txt b/nuttx-configs/px4io-v2/include/README.txt new file mode 100755 index 000000000..2264a80aa --- /dev/null +++ b/nuttx-configs/px4io-v2/include/README.txt @@ -0,0 +1 @@ +This directory contains header files unique to the PX4IO board. diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h new file mode 100755 index 000000000..b93ad4220 --- /dev/null +++ b/nuttx-configs/px4io-v2/include/board.h @@ -0,0 +1,177 @@ +/************************************************************************************ + * configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * 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 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 24000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + +#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#undef GPIO_USART2_CTS +#define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS +#define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK +#define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_TX +#define GPIO_USART3_TX 0xffffffff +#undef GPIO_USART3_CK +#define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS +#define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS +#define GPIO_USART3_RTS 0xffffffff + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ + +/* + * PPM + * + * PPM input is handled by the HRT timer. + * + * Pin is PA8, timer 1, channel 1 + */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs new file mode 100644 index 000000000..bdfc4e3e2 --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4io-v2/common/Make.defs diff --git a/nuttx-configs/px4io-v2/nsh/appconfig b/nuttx-configs/px4io-v2/nsh/appconfig new file mode 100644 index 000000000..48a41bcdb --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/appconfig @@ -0,0 +1,32 @@ +############################################################################ +# +# 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. +# +############################################################################ diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig new file mode 100755 index 000000000..846ea8fb9 --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -0,0 +1,526 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=n +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx-configs/px4io-v2/nsh/setenv.sh b/nuttx-configs/px4io-v2/nsh/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v2/src/Makefile b/nuttx-configs/px4io-v2/src/Makefile new file mode 100644 index 000000000..bb9539d16 --- /dev/null +++ b/nuttx-configs/px4io-v2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/stm3210e-eval/src/Makefile +# +# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx-configs/px4io-v2/src/README.txt b/nuttx-configs/px4io-v2/src/README.txt new file mode 100644 index 000000000..d4eda82fd --- /dev/null +++ b/nuttx-configs/px4io-v2/src/README.txt @@ -0,0 +1 @@ +This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4io-v2/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4io-v2/test/Make.defs b/nuttx-configs/px4io-v2/test/Make.defs new file mode 100644 index 000000000..87508e22e --- /dev/null +++ b/nuttx-configs/px4io-v2/test/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs diff --git a/nuttx-configs/px4io-v2/test/appconfig b/nuttx-configs/px4io-v2/test/appconfig new file mode 100644 index 000000000..3cfc41b43 --- /dev/null +++ b/nuttx-configs/px4io-v2/test/appconfig @@ -0,0 +1,44 @@ +############################################################################ +# configs/stm3210e-eval/nsh/appconfig +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +CONFIGURED_APPS += system/readline +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += reboot + +CONFIGURED_APPS += drivers/boards/px4iov2 diff --git a/nuttx-configs/px4io-v2/test/defconfig b/nuttx-configs/px4io-v2/test/defconfig new file mode 100755 index 000000000..19dfefdf8 --- /dev/null +++ b/nuttx-configs/px4io-v2/test/defconfig @@ -0,0 +1,544 @@ +############################################################################ +# configs/px4io/nsh/defconfig +# +# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip familyl. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_DRAM_END - Last address+1 of installed RAM +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH=arm +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP=stm32 +CONFIG_ARCH_CHIP_STM32F100C8=y +CONFIG_ARCH_BOARD=px4iov2 +CONFIG_ARCH_BOARD_PX4IO_V2=y +CONFIG_BOARD_LOOPSPERMSEC=24000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=y +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=n +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# AHB: +CONFIG_STM32_DMA1=n +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + +# +# Timer and I2C devices may need to the following to force power to be applied: +# +#CONFIG_STM32_FORCEPOWER=y + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=57600 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_NXFLAT. Enable support for the NXFLAT binary format. +# This format will support execution of NuttX binaries located +# in a ROMFS filesystem (see examples/nxflat). +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=n +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=0 +CONFIG_START_YEAR=2009 +CONFIG_START_MONTH=9 +CONFIG_START_DAY=21 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_NXFLAT=n +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=(50*1000) +CONFIG_SCHED_WORKSTACKSIZE=512 +CONFIG_SIG_SIGWORK=4 + +CONFIG_USER_ENTRYPOINT="nsh_main" +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=6 +CONFIG_NFILE_STREAMS=4 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=1 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=3 +CONFIG_PREALLOC_TIMERS=1 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_BUILTIN=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=64 +CONFIG_NSH_STRERROR=n +CONFIG_NSH_LINELEN=64 +CONFIG_NSH_NESTDEPTH=1 +CONFIG_NSH_DISABLESCRIPT=y +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=n +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=n +CONFIG_NSH_IOBUFFER_SIZE=256 +CONFIG_NSH_STACKSIZE=1024 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=n +CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) +CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) +CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0) +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=64 +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +CONFIG_NSH_MMCSDSPIPORTNO=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=800 +CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_PTHREAD_STACK_MIN=256 +CONFIG_PTHREAD_STACK_DEFAULT=512 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx-configs/px4io-v2/test/setenv.sh b/nuttx-configs/px4io-v2/test/setenv.sh new file mode 100755 index 000000000..d83685192 --- /dev/null +++ b/nuttx-configs/px4io-v2/test/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4fmuv2/Kconfig b/nuttx/configs/px4fmuv2/Kconfig deleted file mode 100644 index a10a02ba4..000000000 --- a/nuttx/configs/px4fmuv2/Kconfig +++ /dev/null @@ -1,7 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4FMUV2 -endif diff --git a/nuttx/configs/px4fmuv2/common/Make.defs b/nuttx/configs/px4fmuv2/common/Make.defs deleted file mode 100644 index be387dce1..000000000 --- a/nuttx/configs/px4fmuv2/common/Make.defs +++ /dev/null @@ -1,184 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMUV2 -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard - - -# enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 - -# pull in *just* libm from the toolchain ... this is grody -LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" -EXTRA_LIBS += $(LIBM) - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) \ - -Wno-psabi -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx/configs/px4fmuv2/common/ld.script b/nuttx/configs/px4fmuv2/common/ld.script deleted file mode 100644 index 1be39fb87..000000000 --- a/nuttx/configs/px4fmuv2/common/ld.script +++ /dev/null @@ -1,150 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/common/ld.script - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and - * 256Kb of SRAM. SRAM is split up into three blocks: - * - * 1) 112Kb of SRAM beginning at address 0x2000:0000 - * 2) 16Kb of SRAM beginning at address 0x2001:c000 - * 3) 64Kb of SRAM beginning at address 0x2002:0000 - * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 - * - * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 - * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address range. - * - * The first 0x4000 of flash is reserved for the bootloader. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K - ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K -} - -OUTPUT_ARCH(arm) - -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - - /* - * This is a hack to make the newlib libm __errno() call - * use the NuttX get_errno_ptr() function. - */ - __errno = get_errno_ptr; - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx/configs/px4fmuv2/include/board.h b/nuttx/configs/px4fmuv2/include/board.h deleted file mode 100755 index 0055d65e1..000000000 --- a/nuttx/configs/px4fmuv2/include/board.h +++ /dev/null @@ -1,364 +0,0 @@ -/************************************************************************************ - * configs/px4fmu/include/board.h - * include/arch/board/board.h - * - * Copyright (C) 2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ************************************************************************************/ - -#ifndef __ARCH_BOARD_BOARD_H -#define __ARCH_BOARD_BOARD_H - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include -#ifndef __ASSEMBLY__ -# include -#endif - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/* Clocking *************************************************************************/ -/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. - * - * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: - * System Clock source : PLL (HSE) - * SYSCLK(Hz) : 168000000 Determined by PLL configuration - * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) - * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) - * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) - * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) - * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) - * PLLM : 24 (STM32_PLLCFG_PLLM) - * PLLN : 336 (STM32_PLLCFG_PLLN) - * PLLP : 2 (STM32_PLLCFG_PLLP) - * PLLQ : 7 (STM32_PLLCFG_PPQ) - * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK - * Flash Latency(WS) : 5 - * Prefetch Buffer : OFF - * Instruction cache : ON - * Data cache : ON - * Require 48MHz for USB OTG FS, : Enabled - * SDIO and RNG clock - */ - -/* HSI - 16 MHz RC factory-trimmed - * LSI - 32 KHz RC - * HSE - On-board crystal frequency is 24MHz - * LSE - not installed - */ - -#define STM32_BOARD_XTAL 24000000ul - -#define STM32_HSI_FREQUENCY 16000000ul -#define STM32_LSI_FREQUENCY 32000 -#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL -//#define STM32_LSE_FREQUENCY 32768 - -/* Main PLL Configuration. - * - * PLL source is HSE - * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN - * = (25,000,000 / 25) * 336 - * = 336,000,000 - * SYSCLK = PLL_VCO / PLLP - * = 336,000,000 / 2 = 168,000,000 - * USB OTG FS, SDIO and RNG Clock - * = PLL_VCO / PLLQ - * = 48,000,000 - */ - -#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) -#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) -#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 -#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) - -#define STM32_SYSCLK_FREQUENCY 168000000ul - -/* AHB clock (HCLK) is SYSCLK (168MHz) */ - -#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ -#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY -#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ - -/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ - -#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ -#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) - -/* Timers driven from APB1 will be twice PCLK1 */ - -#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) - -/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ - -#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ -#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) - -/* Timers driven from APB2 will be twice PCLK2 */ - -#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) - -/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. - * Note: TIM1,8 are on APB2, others on APB1 - */ - -#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) -#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) - -/* SDIO dividers. Note that slower clocking is required when DMA is disabled - * in order to avoid RX overrun/TX underrun errors due to delayed responses - * to service FIFOs in interrupt driven mode. These values have not been - * tuned!!! - * - * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz - */ - -#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) - -/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz - * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz - */ - -#ifdef CONFIG_SDIO_DMA -# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) -#else -# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) -#endif - -/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz - * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz - */ - -#ifdef CONFIG_SDIO_DMA -# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) -#else -# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) -#endif - -/* DMA Channl/Stream Selections *****************************************************/ -/* Stream selections are arbitrary for now but might become important in the future - * is we set aside more DMA channels/streams. - * - * SDIO DMA - *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA - *   DMAMAP_SDIO_2 = Channel 4, Stream 6 - */ - -#define DMAMAP_SDIO DMAMAP_SDIO_1 - -/* High-resolution timer - */ -#ifdef CONFIG_HRT_TIMER -# define HRT_TIMER 8 /* use timer8 for the HRT */ -# define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ -#endif - -/* Alternate function pin selections ************************************************/ - -/* - * UARTs. - */ -#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ -#define GPIO_USART1_TX 0 /* USART1 is RX-only */ - -#define GPIO_USART2_RX GPIO_USART2_RX_2 -#define GPIO_USART2_TX GPIO_USART2_TX_2 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 - -#define GPIO_USART3_RX GPIO_USART3_RX_3 -#define GPIO_USART3_TX GPIO_USART3_TX_3 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 - -#define GPIO_UART4_RX GPIO_UART4_RX_1 -#define GPIO_UART4_TX GPIO_UART4_TX_1 - -#define GPIO_USART6_RX GPIO_USART6_RX_1 -#define GPIO_USART6_TX GPIO_USART6_TX_1 - -#define GPIO_UART7_RX GPIO_UART7_RX_1 -#define GPIO_UART7_TX GPIO_UART7_TX_1 - -/* UART8 has no alternate pin config */ - -/* UART RX DMA configurations */ -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 -#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 - -/* - * PWM - * - * Six PWM outputs are configured. - * - * Pins: - * - * CH1 : PE14 : TIM1_CH4 - * CH2 : PE13 : TIM1_CH3 - * CH3 : PE11 : TIM1_CH2 - * CH4 : PE9 : TIM1_CH1 - * CH5 : PD13 : TIM4_CH2 - * CH6 : PD14 : TIM4_CH3 - * - */ -#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 -#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 -#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 -#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 -#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 -#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 - -/* - * CAN - * - * CAN1 is routed to the onboard transceiver. - * CAN2 is routed to the expansion connector. - */ - -#define GPIO_CAN1_RX GPIO_CAN1_RX_3 -#define GPIO_CAN1_TX GPIO_CAN1_TX_3 -#define GPIO_CAN2_RX GPIO_CAN2_RX_1 -#define GPIO_CAN2_TX GPIO_CAN2_TX_2 - -/* - * I2C - * - * The optional _GPIO configurations allow the I2C driver to manually - * reset the bus to clear stuck slaves. They match the pin configuration, - * but are normally-high GPIOs. - */ -#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 -#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 -#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) -#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) - -#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 -#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 -#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) -#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) - -/* - * I2C busses - */ -#define PX4_I2C_BUS_EXPANSION 1 -#define PX4_I2C_BUS_LED 2 - -/* - * Devices on the onboard bus. - * - * Note that these are unshifted addresses. - */ -#define PX4_I2C_OBDEV_LED 0x55 -#define PX4_I2C_OBDEV_HMC5883 0x1e - -/* - * SPI - * - * There are sensors on SPI1, and SPI2 is connected to the FRAM. - */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 - -#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 -#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 - -/* - * Use these in place of the spi_dev_e enumeration to - * select a specific SPI device on SPI1 - */ -#define PX4_SPIDEV_GYRO 1 -#define PX4_SPIDEV_ACCEL_MAG 2 -#define PX4_SPIDEV_BARO 3 - -/* - * Tone alarm output - */ -#define TONE_ALARM_TIMER 2 /* timer 2 */ -#define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) - -/************************************************************************************ - * Public Data - ************************************************************************************/ - -#ifndef __ASSEMBLY__ - -#undef EXTERN -#if defined(__cplusplus) -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -/************************************************************************************ - * Public Function Prototypes - ************************************************************************************/ -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -EXTERN void stm32_boardinitialize(void); - -#undef EXTERN -#if defined(__cplusplus) -} -#endif - -#endif /* __ASSEMBLY__ */ -#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h b/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h deleted file mode 100644 index 15e4e7a8d..000000000 --- a/nuttx/configs/px4fmuv2/include/nsh_romfsimg.h +++ /dev/null @@ -1,42 +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. - * - ****************************************************************************/ - -/** - * nsh_romfsetc.h - * - * This file is a stub for 'make export' purposes; the actual ROMFS - * must be supplied by the library client. - */ - -extern unsigned char romfs_img[]; -extern unsigned int romfs_img_len; diff --git a/nuttx/configs/px4fmuv2/nsh/Make.defs b/nuttx/configs/px4fmuv2/nsh/Make.defs deleted file mode 100644 index 3306e4bf1..000000000 --- a/nuttx/configs/px4fmuv2/nsh/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/px4fmuv2/common/Make.defs diff --git a/nuttx/configs/px4fmuv2/nsh/appconfig b/nuttx/configs/px4fmuv2/nsh/appconfig deleted file mode 100644 index 0e18aa8ef..000000000 --- a/nuttx/configs/px4fmuv2/nsh/appconfig +++ /dev/null @@ -1,52 +0,0 @@ -############################################################################ -# configs/px4fmu/nsh/appconfig -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# Path to example in apps/examples containing the user_start entry point - -CONFIGURED_APPS += examples/nsh - -# The NSH application library -CONFIGURED_APPS += nshlib -CONFIGURED_APPS += system/readline - -ifeq ($(CONFIG_CAN),y) -#CONFIGURED_APPS += examples/can -endif - -#ifeq ($(CONFIG_USBDEV),y) -#ifeq ($(CONFIG_CDCACM),y) -CONFIGURED_APPS += examples/cdcacm -#endif -#endif diff --git a/nuttx/configs/px4fmuv2/nsh/defconfig b/nuttx/configs/px4fmuv2/nsh/defconfig deleted file mode 100755 index 17803c4d7..000000000 --- a/nuttx/configs/px4fmuv2/nsh/defconfig +++ /dev/null @@ -1,1083 +0,0 @@ -############################################################################ -# configs/px4fmu/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization -# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM4=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_BOARD="px4fmuv2" -CONFIG_ARCH_BOARD_PX4FMUV2=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_DRAM_SIZE=0x00040000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_FPU=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y -CONFIG_ARCH_MATH_H=y - -CONFIG_ARMV7M_CMNVECTOR=y -CONFIG_STM32_STM32F427=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y - -# -# On-chip CCM SRAM configuration -# -# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need -# to do this if DMA is enabled to prevent non-DMA-able CCM memory from -# being a part of the stack. -# -CONFIG_STM32_CCMEXCLUDE=y - -# -# On-board FSMC SRAM configuration -# -# CONFIG_STM32_FSMC - Required. See below -# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) -# -# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the -# FSMC (as opposed to an LCD or FLASH). -# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space -# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space -# -#CONFIG_STM32_FSMC_SRAM=n -#CONFIG_HEAP2_BASE=0x64000000 -#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) - -# -# Individual subsystems can be enabled: -# -# This set is exhaustive for PX4FMU and should be safe to cut and -# paste into any other config. -# -# AHB1: -CONFIG_STM32_CRC=n -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_ETHMAC=n -CONFIG_STM32_OTGHS=n -# AHB2: -CONFIG_STM32_DCMI=n -CONFIG_STM32_CRYP=n -CONFIG_STM32_HASH=n -CONFIG_STM32_RNG=n -CONFIG_STM32_OTGFS=y -# AHB3: -CONFIG_STM32_FSMC=n -# APB1: -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=y -CONFIG_STM32_TIM4=y -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_TIM12=n -CONFIG_STM32_TIM13=n -CONFIG_STM32_TIM14=n -CONFIG_STM32_WWDG=y -CONFIG_STM32_IWDG=n -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI3=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART5=n -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=y -CONFIG_STM32_I2C3=n -CONFIG_STM32_CAN1=n -CONFIG_STM32_CAN2=n -CONFIG_STM32_DAC=n -CONFIG_STM32_PWR=y -# APB2: -CONFIG_STM32_TIM1=y -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -# Mostly owned by the px4io driver, but uploader needs this -CONFIG_STM32_USART6=y -# We use our own driver, but leave this on. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -CONFIG_STM32_ADC3=n -CONFIG_STM32_SDIO=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SYSCFG=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y - -# -# Enable single wire support. If this is not defined, then this mode cannot -# be enabled. -# -CONFIG_STM32_USART_SINGLEWIRE=y - -# -# We want the flash prefetch on for max performance. -# -STM32_FLASH_PREFETCH=y - -# -# STM32F40xxx specific serial device driver settings -# -# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, -# tcflush, etc.). If this is not defined, then the terminal settings (baud, -# parity, etc.) are not configurable at runtime; serial streams cannot be -# flushed, etc. -# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port -# immediately after creating the /dev/console device. This is required -# if the console serial port has RX DMA enabled. -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_CONSOLE_REINIT=n -CONFIG_STANDARD_SERIAL=y - -CONFIG_UART8_SERIAL_CONSOLE=y - -#Mavlink messages can be bigger than 128 -CONFIG_USART1_TXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=256 -CONFIG_USART3_TXBUFSIZE=256 -CONFIG_UART4_TXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=128 -CONFIG_UART7_TXBUFSIZE=256 -CONFIG_UART8_TXBUFSIZE=256 - -CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART2_RXBUFSIZE=256 -CONFIG_USART3_RXBUFSIZE=256 -CONFIG_UART4_RXBUFSIZE=256 -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_UART7_RXBUFSIZE=256 -CONFIG_UART8_RXBUFSIZE=256 - -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 -CONFIG_UART4_BAUD=115200 -CONFIG_USART6_BAUD=115200 -CONFIG_UART7_BAUD=115200 -CONFIG_UART8_BAUD=57600 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 -CONFIG_UART4_BITS=8 -CONFIG_USART6_BITS=8 -CONFIG_UART7_BITS=8 -CONFIG_UART8_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 -CONFIG_UART4_PARITY=0 -CONFIG_USART6_PARITY=0 -CONFIG_UART7_PARITY=0 -CONFIG_UART8_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 -CONFIG_UART4_2STOP=0 -CONFIG_USART6_2STOP=0 -CONFIG_UART7_2STOP=0 -CONFIG_UART8_2STOP=0 - -CONFIG_USART1_RXDMA=y -CONFIG_USART2_RXDMA=y -CONFIG_USART3_RXDMA=n -CONFIG_UART4_RXDMA=n -CONFIG_USART6_RXDMA=y -CONFIG_UART7_RXDMA=n -CONFIG_UART8_RXDMA=n - -# -# PX4FMU specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=n - -# -# STM32F40xxx specific SPI device driver settings -# -CONFIG_SPI_EXCHANGE=y -# DMA needs more work, not implemented on STM32F4x yet -#CONFIG_STM32_SPI_DMA=y - -# -# STM32F40xxx specific CAN device driver settings -# -# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or -# CONFIG_STM32_CAN2 must also be defined) -# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default -# Standard 11-bit IDs. -# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. -# Default: 8 -# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. -# Default: 4 -# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback -# mode for testing. The STM32 CAN driver does support loopback mode. -# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. -# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. -# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 -# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 -# -CONFIG_CAN=n -CONFIG_CAN_EXTID=n -#CONFIG_CAN_FIFOSIZE -#CONFIG_CAN_NPENDINGRTR -CONFIG_CAN_LOOPBACK=n -CONFIG_CAN1_BAUD=700000 -CONFIG_CAN2_BAUD=700000 - - -# XXX remove after integration testing -# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using -# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update -CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 -# Constant overhead for generating I2C start / stop conditions -CONFIG_STM32_I2CTIMEOUS_START_STOP=700 -# XXX this is bad and we want it gone -CONFIG_I2C_WRITEREAD=y - -# -# I2C configuration -# -CONFIG_I2C=y -CONFIG_I2C_POLLED=n -CONFIG_I2C_TRANSFER=y -CONFIG_I2C_TRACE=n -CONFIG_I2C_RESET=y -# XXX fixed per-transaction timeout -CONFIG_STM32_I2CTIMEOMS=10 - -# -# MTD support -# -CONFIG_MTD=y - -# XXX re-enable after integration testing - -# -# I2C configuration -# -#CONFIG_I2C=y -#CONFIG_I2C_POLLED=y -#CONFIG_I2C_TRANSFER=y -#CONFIG_I2C_TRACE=n -#CONFIG_I2C_RESET=y - -# Dynamic timeout -#CONFIG_STM32_I2C_DYNTIMEO=y -#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 -#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 - -# Fixed per-transaction timeout -#CONFIG_STM32_I2CTIMEOSEC=0 -#CONFIG_STM32_I2CTIMEOMS=10 - - - - - - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=y - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 192 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# -# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single -# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined -# then an additional, lower-priority work queue will also be created. This -# lower priority work queue is better suited for more extended processing -# (such as file system clean-up operations) -# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority -# worker thread. Default: 50 -# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread -# checks for work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower -# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API -# -CONFIG_USER_ENTRYPOINT="nsh_main" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=y -CONFIG_DEBUG_VERBOSE=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=2 -CONFIG_ARCH_LOWPUTC=y -CONFIG_MSEC_PER_TICK=1 -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=y -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=8 -CONFIG_FDCLONE_DISABLE=n -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=y -CONFIG_SCHED_WORKPRIORITY=192 -CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=y -CONFIG_SCHED_ATEXIT=n - -# -# System Logging -# -# CONFIG_SYSLOG - Enables the System Logging feature. -# CONFIG_RAMLOG - Enables the RAM logging feature -# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. -# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all -# console output will be re-directed to a circular buffer in RAM. This -# is useful, for example, if the only console is a Telnet console. Then -# in that case, console output from non-Telnet threads will go to the -# circular buffer and can be viewed using the NSH 'dmesg' command. -# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging -# interface. If this feature is enabled (along with CONFIG_SYSLOG), -# then all debug output (only) will be re-directed to the circular -# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' -# command. -# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting -# for this driver on poll(). Default: 4 -# -# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the -# following may also be provided: -# -# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 -# - -CONFIG_SYSLOG=n -CONFIG_RAMLOG=n -CONFIG_RAMLOG_CONSOLE=n -CONFIG_RAMLOG_SYSLOG=n -#CONFIG_RAMLOG_NPOLLWAITERS -#CONFIG_RAMLOG_CONSOLE_BUFSIZE - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=n -CONFIG_DISABLE_PTHREAD=n -CONFIG_DISABLE_SIGNALS=n -CONFIG_DISABLE_MQUEUE=n -CONFIG_DISABLE_MOUNTPOINT=n -CONFIG_DISABLE_ENVIRON=n -CONFIG_DISABLE_POLL=n - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") -# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: -# 5.1234567 -# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) -# -CONFIG_NOPRINTF_FIELDWIDTH=n -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_HAVE_LONG_LONG=y - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=y -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=32 -CONFIG_MAX_TASK_ARGS=8 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 -CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=256 -CONFIG_STDIO_LINEBUFFER=y -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=50 -CONFIG_PREALLOC_TIMERS=50 - -# -# Filesystem configuration -# -# CONFIG_FS_FAT - Enable FAT filesystem support -# CONFIG_FAT_SECTORSIZE - Max supported sector size -# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 -# file name support. -# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims -# patents on FAT long file name technology. Please read the -# disclaimer in the top-level COPYING file and only enable this -# feature if you understand these issues. -# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the -# default, maximum long file name is 255 bytes. This can eat up -# a lot of memory (especially stack space). If you are willing -# to live with some non-standard, short long file names, then -# define this value. A good choice would be the same value as -# selected for CONFIG_NAME_MAX which will limit the visibility -# of longer file names anyway. -# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. -# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. -# This must have one of the values of 0xff or 0x00. -# Default: 0xff. -# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, -# don't both with file chunks smaller than this number of data bytes. -# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. -# Default: 255. -# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, -# don't both with file chunks smaller than this number of data bytes. -# Default: 32. -# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean -# packing files together toward the end of the file or, if file are -# deleted at the end of the file, clean up can simply mean erasing -# the end of FLASH memory so that it can be re-used again. However, -# doing this can also harm the life of the FLASH part because it can -# mean that the tail end of the FLASH is re-used too often. This -# threshold determines if/when it is worth erased the tail end of FLASH -# and making it available for re-use (and possible over-wear). -# Default: 8192. -# CONFIG_FS_ROMFS - Enable ROMFS filesystem support -# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this -# option will enable a limited form of memory mapping that is -# implemented by copying whole files into memory. -# -CONFIG_FS_FAT=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_MAXFNAME=32 -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_FS_ROMFS=y -CONFIG_FS_BINFS=y - -# -# SPI-based MMC/SD driver -# -# CONFIG_MMCSD_NSLOTS -# Number of MMC/SD slots supported by the driver -# CONFIG_MMCSD_READONLY -# Provide read-only access (default is read/write) -# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. -# Default is 20MHz, current setting 24 MHz -# -#CONFIG_MMCSD=n -# XXX need to rejig this for SDIO -#CONFIG_MMCSD_SPI=y -#CONFIG_MMCSD_NSLOTS=1 -#CONFIG_MMCSD_READONLY=n -#CONFIG_MMCSD_SPICLOCK=24000000 - -# -# Maintain legacy build behavior (revisit) -# - -CONFIG_MMCSD=y -#CONFIG_MMCSD_SPI=y -CONFIG_MMCSD_SDIO=y -CONFIG_MTD=y - -# -# SPI-based MMC/SD driver -# -#CONFIG_MMCSD_NSLOTS=1 -#CONFIG_MMCSD_READONLY=n -#CONFIG_MMCSD_SPICLOCK=12500000 - -# -# STM32 SDIO-based MMC/SD driver -# -CONFIG_SDIO_DMA=y -#CONFIG_SDIO_PRI=128 -#CONFIG_SDIO_DMAPRIO -#CONFIG_SDIO_WIDTH_D1_ONLY -CONFIG_MMCSD_MULTIBLOCK_DISABLE=y -CONFIG_MMCSD_MMCSUPPORT=n -CONFIG_MMCSD_HAVECARDDETECT=n - -# -# Block driver buffering -# -# CONFIG_FS_READAHEAD -# Enable read-ahead buffering -# CONFIG_FS_WRITEBUFFER -# Enable write buffering -# -CONFIG_FS_READAHEAD=n -CONFIG_FS_WRITEBUFFER=n - -# -# RTC Configuration -# -# CONFIG_RTC - Enables general support for a hardware RTC. Specific -# architectures may require other specific settings. -# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple -# battery backed counter that keeps the time when power is down, and (2) -# A full date / time RTC the provides the date and time information, often -# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this -# second kind of RTC. In this case, the RTC is used to "seed" the normal -# NuttX timer and the NuttX system timer provides for higher resoution -# time. -# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, -# battery backed counter is used. There are two different implementations -# of such simple counters based on the time resolution of the counter: -# The typical RTC keeps time to resolution of 1 second, usually -# supporting a 32-bit time_t value. In this case, the RTC is used to -# "seed" the normal NuttX timer and the NuttX timer provides for higher -# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, -# then the RTC provides higher resolution time and completely replaces the -# system timer for purpose of date and time. -# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency -# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is -# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. -# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an -# alarm. A callback function will be executed when the alarm goes off -# -CONFIG_RTC=n -CONFIG_RTC_DATETIME=y -CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n -CONFIG_RTC_ALARM=n - -# -# USB Device Configuration -# -# CONFIG_USBDEV -# Enables USB device support -# CONFIG_USBDEV_ISOCHRONOUS -# Build in extra support for isochronous endpoints -# CONFIG_USBDEV_DUALSPEED -# Hardware handles high and full speed operation (USB 2.0) -# CONFIG_USBDEV_SELFPOWERED -# Will cause USB features to indicate that the device is -# self-powered -# CONFIG_USBDEV_MAXPOWER -# Maximum power consumption in mA -# CONFIG_USBDEV_TRACE -# Enables USB tracing for debug -# CONFIG_USBDEV_TRACE_NRECORDS -# Number of trace entries to remember -# -CONFIG_USBDEV=y -CONFIG_USBDEV_ISOCHRONOUS=n -CONFIG_USBDEV_DUALSPEED=n -CONFIG_USBDEV_SELFPOWERED=y -CONFIG_USBDEV_REMOTEWAKEUP=n -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USBDEV_TRACE=n -CONFIG_USBDEV_TRACE_NRECORDS=512 - -# -# USB serial device class driver (Standard CDC ACM class) -# -# CONFIG_CDCACM -# Enable compilation of the USB serial driver -# CONFIG_CDCACM_CONSOLE -# Configures the CDC/ACM serial port as the console device. -# CONFIG_CDCACM_EP0MAXPACKET -# Endpoint 0 max packet size. Default 64 -# CONFIG_CDCACM_EPINTIN -# The logical 7-bit address of a hardware endpoint that supports -# interrupt IN operation. Default 2. -# CONFIG_CDCACM_EPINTIN_FSSIZE -# Max package size for the interrupt IN endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPINTIN_HSSIZE -# Max package size for the interrupt IN endpoint if high speed mode. -# Default 64 -# CONFIG_CDCACM_EPBULKOUT -# The logical 7-bit address of a hardware endpoint that supports -# bulk OUT operation. Default 4. -# CONFIG_CDCACM_EPBULKOUT_FSSIZE -# Max package size for the bulk OUT endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPBULKOUT_HSSIZE -# Max package size for the bulk OUT endpoint if high speed mode. -# Default 512. -# CONFIG_CDCACM_EPBULKIN -# The logical 7-bit address of a hardware endpoint that supports -# bulk IN operation. Default 3. -# CONFIG_CDCACM_EPBULKIN_FSSIZE -# Max package size for the bulk IN endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPBULKIN_HSSIZE -# Max package size for the bulk IN endpoint if high speed mode. -# Default 512. -# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS -# The number of write/read requests that can be in flight. -# Default 256. -# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR -# The vendor ID code/string. Default 0x0525 and "NuttX" -# 0x0525 is the Netchip vendor and should not be used in any -# products. This default VID was selected for compatibility with -# the Linux CDC ACM default VID. -# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR -# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" -# 0xa4a7 was selected for compatibility with the Linux CDC ACM -# default PID. -# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE -# Size of the serial receive/transmit buffers. Default 256. -# -CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n -#CONFIG_CDCACM_EP0MAXPACKET -CONFIG_CDCACM_EPINTIN=1 -#CONFIG_CDCACM_EPINTIN_FSSIZE -#CONFIG_CDCACM_EPINTIN_HSSIZE -CONFIG_CDCACM_EPBULKOUT=3 -#CONFIG_CDCACM_EPBULKOUT_FSSIZE -#CONFIG_CDCACM_EPBULKOUT_HSSIZE -CONFIG_CDCACM_EPBULKIN=2 -#CONFIG_CDCACM_EPBULKIN_FSSIZE -#CONFIG_CDCACM_EPBULKIN_HSSIZE -#CONFIG_CDCACM_NWRREQS -#CONFIG_CDCACM_NRDREQS -CONFIG_CDCACM_VENDORID=0x26AC -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTID=0x0010 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" -#CONFIG_CDCACM_RXBUFSIZE -#CONFIG_CDCACM_TXBUFSIZE - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# -# If CONFIG_NSH_TELNET is selected: -# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size -# CONFIG_NSH_DHCPC - Obtain address using DHCP -# CONFIG_NSH_IPADDR - Provides static IP address -# CONFIG_NSH_DRIPADDR - Provides static router IP address -# CONFIG_NSH_NETMASK - Provides static network mask -# CONFIG_NSH_NOMAC - Use a bogus MAC address -# -# If CONFIG_NSH_ROMFSETC is selected: -# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint -# CONFIG_NSH_INITSCRIPT - Relative path to init script -# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor -# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size -# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor -# CONFIG_NSH_FATSECTSIZE - FAT FS sector size -# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors -# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint -# -CONFIG_BUILTIN=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_FILEIOSIZE=512 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAX_ARGUMENTS=12 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_DISABLESCRIPT=n -CONFIG_NSH_DISABLEBG=n -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_CONSOLE=y -CONFIG_NSH_USBCONSOLE=n -#CONFIG_NSH_USBCONDEV="/dev/ttyACM0" -CONFIG_NSH_TELNET=n -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_IOBUFFER_SIZE=512 -CONFIG_NSH_DHCPC=n -CONFIG_NSH_NOMAC=y -CONFIG_NSH_IPADDR=0x0a000002 -CONFIG_NSH_DRIPADDR=0x0a000001 -CONFIG_NSH_NETMASK=0xffffff00 -CONFIG_NSH_ROMFSMOUNTPT="/etc" -CONFIG_NSH_INITSCRIPT="init.d/rcS" -CONFIG_NSH_ROMFSDEVNO=0 -CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard -CONFIG_NSH_FATDEVNO=1 -CONFIG_NSH_FATSECTSIZE=512 -CONFIG_NSH_FATNSECTORS=1024 -CONFIG_NSH_FATMOUNTPT=/tmp - -# -# Architecture-specific NSH options -# -#CONFIG_NSH_MMCSDSPIPORTNO=3 -CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDMINOR=0 - - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3240G-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -# Idle thread needs 4096 bytes -# default 1 KB is not enough -# 4096 bytes -CONFIG_IDLETHREAD_STACKSIZE=6000 -# USERMAIN stack size probably needs to be around 4096 bytes -CONFIG_USERMAIN_STACKSIZE=4096 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=2048 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= - -# enable bindir -CONFIG_APPS_BINDIR=y diff --git a/nuttx/configs/px4fmuv2/nsh/setenv.sh b/nuttx/configs/px4fmuv2/nsh/setenv.sh deleted file mode 100755 index 265520997..000000000 --- a/nuttx/configs/px4fmuv2/nsh/setenv.sh +++ /dev/null @@ -1,67 +0,0 @@ -#!/bin/bash -# configs/stm3240g-eval/nsh/setenv.sh -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$_" = "$0" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -WD=`pwd` -if [ ! -x "setenv.sh" ]; then - echo "This script must be executed from the top-level NuttX build directory" - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then - export PATH_ORIG="${PATH}" -fi - -# This the Cygwin path to the location where I installed the RIDE -# toolchain under windows. You will also have to edit this if you install -# the RIDE toolchain in any other location -#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" - -# This the Cygwin path to the location where I installed the CodeSourcery -# toolchain under windows. You will also have to edit this if you install -# the CodeSourcery toolchain in any other location -export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" - -# This the Cygwin path to the location where I build the buildroot -# toolchain. -#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" - -# Add the path to the toolchain to the PATH varialble -export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4fmuv2/src/Makefile b/nuttx/configs/px4fmuv2/src/Makefile deleted file mode 100644 index d4276f7fc..000000000 --- a/nuttx/configs/px4fmuv2/src/Makefile +++ /dev/null @@ -1,84 +0,0 @@ -############################################################################ -# configs/px4fmu/src/Makefile -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - --include $(TOPDIR)/Make.defs - -CFLAGS += -I$(TOPDIR)/sched - -ASRCS = -AOBJS = $(ASRCS:.S=$(OBJEXT)) - -CSRCS = empty.c -COBJS = $(CSRCS:.c=$(OBJEXT)) - -SRCS = $(ASRCS) $(CSRCS) -OBJS = $(AOBJS) $(COBJS) - -ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src -ifeq ($(WINTOOL),y) - CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ - -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ - -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" -else - CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m -endif - -all: libboard$(LIBEXT) - -$(AOBJS): %$(OBJEXT): %.S - $(call ASSEMBLE, $<, $@) - -$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c - $(call COMPILE, $<, $@) - -libboard$(LIBEXT): $(OBJS) - $(call ARCHIVE, $@, $(OBJS)) - -.depend: Makefile $(SRCS) - $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - $(Q) touch $@ - -depend: .depend - -clean: - $(call DELFILE, libboard$(LIBEXT)) - $(call CLEAN) - -distclean: clean - $(call DELFILE, Make.dep) - $(call DELFILE, .depend) - --include Make.dep - diff --git a/nuttx/configs/px4fmuv2/src/empty.c b/nuttx/configs/px4fmuv2/src/empty.c deleted file mode 100644 index ace900866..000000000 --- a/nuttx/configs/px4fmuv2/src/empty.c +++ /dev/null @@ -1,4 +0,0 @@ -/* - * There are no source files here, but libboard.a can't be empty, so - * we have this empty source file to keep it company. - */ diff --git a/nuttx/configs/px4iov2/README.txt b/nuttx/configs/px4iov2/README.txt deleted file mode 100755 index 9b1615f42..000000000 --- a/nuttx/configs/px4iov2/README.txt +++ /dev/null @@ -1,806 +0,0 @@ -README -====== - -This README discusses issues unique to NuttX configurations for the -STMicro STM3210E-EVAL development board. - -Contents -======== - - - Development Environment - - GNU Toolchain Options - - IDEs - - NuttX buildroot Toolchain - - DFU and JTAG - - OpenOCD - - LEDs - - Temperature Sensor - - RTC - - STM3210E-EVAL-specific Configuration Options - - Configurations - -Development Environment -======================= - - Either Linux or Cygwin on Windows can be used for the development environment. - The source has been built only using the GNU toolchain (see below). Other - toolchains will likely cause problems. Testing was performed using the Cygwin - environment because the Raisonance R-Link emulatator and some RIDE7 development tools - were used and those tools works only under Windows. - -GNU Toolchain Options -===================== - - The NuttX make system has been modified to support the following different - toolchain options. - - 1. The CodeSourcery GNU toolchain, - 2. The devkitARM GNU toolchain, - 3. Raisonance GNU toolchain, or - 4. The NuttX buildroot Toolchain (see below). - - All testing has been conducted using the NuttX buildroot toolchain. However, - the make system is setup to default to use the devkitARM toolchain. To use - the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to - add one of the following configuration options to your .config (or defconfig) - file: - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux - CONFIG_STM32_DEVKITARM=y : devkitARM under Windows - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) - - If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify - the PATH in the setenv.h file if your make cannot find the tools. - - NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are - Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot - toolchains are Cygwin and/or Linux native toolchains. There are several limitations - to using a Windows based toolchain in a Cygwin environment. The three biggest are: - - 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are - performed automatically in the Cygwin makefiles using the 'cygpath' utility - but you might easily find some new path problems. If so, check out 'cygpath -w' - - 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links - are used in Nuttx (e.g., include/arch). The make system works around these - problems for the Windows tools by copying directories instead of linking them. - But this can also cause some confusion for you: For example, you may edit - a file in a "linked" directory and find that your changes had no effect. - That is because you are building the copy of the file in the "fake" symbolic - directory. If you use a Windows toolchain, you should get in the habit of - making like this: - - make clean_context all - - An alias in your .bashrc file might make that less painful. - - 3. Dependencies are not made when using Windows versions of the GCC. This is - because the dependencies are generated using Windows pathes which do not - work with the Cygwin make. - - Support has been added for making dependencies with the windows-native toolchains. - That support can be enabled by modifying your Make.defs file as follows: - - - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" - - If you have problems with the dependency build (for example, if you are not - building on C:), then you may need to modify tools/mkdeps.sh - - NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization - level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with - -Os. - - NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that - the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM - path or will get the wrong version of make. - -IDEs -==== - - NuttX is built using command-line make. It can be used with an IDE, but some - effort will be required to create the project (There is a simple RIDE project - in the RIDE subdirectory). - - Makefile Build - -------------- - Under Eclipse, it is pretty easy to set up an "empty makefile project" and - simply use the NuttX makefile to build the system. That is almost for free - under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty - makefile project in order to work with Windows (Google for "Eclipse Cygwin" - - there is a lot of help on the internet). - - Native Build - ------------ - Here are a few tips before you start that effort: - - 1) Select the toolchain that you will be using in your .config file - 2) Start the NuttX build at least one time from the Cygwin command line - before trying to create your project. This is necessary to create - certain auto-generated files and directories that will be needed. - 3) Set up include pathes: You will need include/, arch/arm/src/stm32, - arch/arm/src/common, arch/arm/src/armv7-m, and sched/. - 4) All assembly files need to have the definition option -D __ASSEMBLY__ - on the command line. - - Startup files will probably cause you some headaches. The NuttX startup file - is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX - one time from the Cygwin command line in order to obtain the pre-built - startup object needed by RIDE. - -NuttX buildroot Toolchain -========================= - - A GNU GCC-based toolchain is assumed. The files */setenv.sh should - be modified to point to the correct path to the Cortex-M3 GCC toolchain (if - different from the default in your PATH variable). - - If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX - SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). - This GNU toolchain builds and executes in the Linux or Cygwin environment. - - 1. You must have already configured Nuttx in /nuttx. - - cd tools - ./configure.sh stm3210e-eval/ - - 2. Download the latest buildroot package into - - 3. unpack the buildroot tarball. The resulting directory may - have versioning information on it like buildroot-x.y.z. If so, - rename /buildroot-x.y.z to /buildroot. - - 4. cd /buildroot - - 5. cp configs/cortexm3-defconfig-4.3.3 .config - - 6. make oldconfig - - 7. make - - 8. Edit setenv.h, if necessary, so that the PATH variable includes - the path to the newly built binaries. - - See the file configs/README.txt in the buildroot source tree. That has more - detailed PLUS some special instructions that you will need to follow if you are - building a Cortex-M3 toolchain for Cygwin under Windows. - -DFU and JTAG -============ - - Enbling Support for the DFU Bootloader - -------------------------------------- - The linker files in these projects can be configured to indicate that you - will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) - loader or via some JTAG emulator. You can specify the DFU bootloader by - adding the following line: - - CONFIG_STM32_DFU=y - - to your .config file. Most of the configurations in this directory are set - up to use the DFU loader. - - If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning - of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed - to make space for the DFU loader and 0x08003000 is where the DFU loader expects - to find new applications at boot time. If you need to change that origin for some - other bootloader, you will need to edit the file(s) ld.script.dfu for each - configuration. - - The DFU SE PC-based software is available from the STMicro website, - http://www.st.com. General usage instructions: - - 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU - file (nuttx.dfu)... see below for details. - 2. Connect the STM3210E-EVAL board to your computer using a USB - cable. - 3. Start the DFU loader on the STM3210E-EVAL board. You do this by - resetting the board while holding the "Key" button. Windows should - recognize that the DFU loader has been installed. - 3. Run the DFU SE program to load nuttx.dfu into FLASH. - - What if the DFU loader is not in FLASH? The loader code is available - inside of the Demo dirctory of the USBLib ZIP file that can be downloaded - from the STMicro Website. You can build it using RIDE (or other toolchains); - you will need a JTAG emulator to burn it into FLASH the first time. - - In order to use STMicro's built-in DFU loader, you will have to get - the NuttX binary into a special format with a .dfu extension. The - DFU SE PC_based software installation includes a file "DFU File Manager" - conversion program that a file in Intel Hex format to the special DFU - format. When you successfully build NuttX, you will find a file called - nutt.ihx in the top-level directory. That is the file that you should - provide to the DFU File Manager. You will need to rename it to nuttx.hex - in order to find it with the DFU File Manager. You will end up with - a file called nuttx.dfu that you can use with the STMicro DFU SE program. - - Enabling JTAG - ------------- - If you are not using the DFU, then you will probably also need to enable - JTAG support. By default, all JTAG support is disabled but there NuttX - configuration options to enable JTAG in various different ways. - - These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO - MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the - Cortex debug port. The default state in this port is for all JTAG support - to be disable. - - CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full - SWJ (JTAG-DP + SW-DP) - - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable - full SWJ (JTAG-DP + SW-DP) but without JNTRST. - - CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP - disabled and SW-DP enabled - - The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 - which disable JTAG-DP and SW-DP. - -OpenOCD -======= - -I have also used OpenOCD with the STM3210E-EVAL. In this case, I used -the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh -for more information. Using the script: - -1) Start the OpenOCD GDB server - - cd - configs/stm3210e-eval/tools/oocd.sh $PWD - -2) Load Nuttx - - cd - arm-none-eabi-gdb nuttx - gdb> target remote localhost:3333 - gdb> mon reset - gdb> mon halt - gdb> load nuttx - -3) Running NuttX - - gdb> mon reset - gdb> c - -LEDs -==== - -The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the -board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is -defined. In that case, the usage by the board port is defined in -include/board.h and src/up_leds.c. The LEDs are used to encode OS-related -events as follows: - - SYMBOL Meaning LED1* LED2 LED3 LED4 - ---------------- ----------------------- ----- ----- ----- ----- - LED_STARTED NuttX has been started ON OFF OFF OFF - LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF - LED_IRQSENABLED Interrupts enabled ON ON OFF OFF - LED_STACKCREATED Idle stack created OFF OFF ON OFF - LED_INIRQ In an interrupt** ON N/C N/C OFF - LED_SIGNAL In a signal handler*** N/C ON N/C OFF - LED_ASSERTION An assertion failed ON ON N/C OFF - LED_PANIC The system has crashed N/C N/C N/C ON - LED_IDLE STM32 is is sleep mode (Optional, not used) - - * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot - and these LEDs will give you some indication of where the failure was - ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow - is because of timer interupts that result in the LED being illuminated - on a small proportion of the time. -*** LED2 may also flicker normally if signals are processed. - -Temperature Sensor -================== - -Support for the on-board LM-75 temperature sensor is available. This supported -has been verified, but has not been included in any of the available the -configurations. To set up the temperature sensor, add the following to the -NuttX configuration file - - CONFIG_I2C=y - CONFIG_I2C_LM75=y - -Then you can implement logic like the following to use the temperature sensor: - - #include - #include - - ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ - fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ - ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ - bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ - -More complex temperature sensor operations are also available. See the IOCTAL -commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions -of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the -arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). - -RTC -=== - - The STM32 RTC may configured using the following settings. - - CONFIG_RTC - Enables general support for a hardware RTC. Specific - architectures may require other specific settings. - CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 - second, usually supporting a 32-bit time_t value. In this case, - the RTC is used to "seed" the normal NuttX timer and the - NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES - is enabled in the NuttX configuration, then the RTC provides higher - resolution time and completely replaces the system timer for purpose of - date and time. - CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the - frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES - is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. - CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. - A callback function will be executed when the alarm goes off - - In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts - are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. - A BKP register is incremented on each overflow interrupt creating, effectively, - a 48-bit RTC counter. - - In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled - (because the next overflow is not expected until the year 2106. - - WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The - overflow interrupt may be lost even if the STM32 is powered down only momentarily. - Therefore hi-res solution is only useful in systems where the power is always on. - -STM3210E-EVAL-specific Configuration Options -============================================ - - CONFIG_ARCH - Identifies the arch/ subdirectory. This should - be set to: - - CONFIG_ARCH=arm - - CONFIG_ARCH_family - For use in C code: - - CONFIG_ARCH_ARM=y - - CONFIG_ARCH_architecture - For use in C code: - - CONFIG_ARCH_CORTEXM3=y - - CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory - - CONFIG_ARCH_CHIP=stm32 - - CONFIG_ARCH_CHIP_name - For use in C code to identify the exact - chip: - - CONFIG_ARCH_CHIP_STM32F103ZET6 - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock - configuration features. - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n - - CONFIG_ARCH_BOARD - Identifies the configs subdirectory and - hence, the board that supports the particular chip or SoC. - - CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) - - CONFIG_ARCH_BOARD_name - For use in C code - - CONFIG_ARCH_BOARD_STM3210E_EVAL=y - - CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation - of delay loops - - CONFIG_ENDIAN_BIG - define if big endian (default is little - endian) - - CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): - - CONFIG_DRAM_SIZE=0x00010000 (64Kb) - - CONFIG_DRAM_START - The start address of installed DRAM - - CONFIG_DRAM_START=0x20000000 - - CONFIG_DRAM_END - Last address+1 of installed RAM - - CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) - - CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization - - CONFIG_ARCH_IRQPRIO=y - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that - have LEDs - - CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt - stack. If defined, this symbol is the size of the interrupt - stack in bytes. If not defined, the user task stacks will be - used during interrupt handling. - - CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. - - CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that - cause a 100 second delay during boot-up. This 100 second delay - serves no purpose other than it allows you to calibratre - CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure - the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until - the delay actually is 100 seconds. - - Individual subsystems can be enabled: - AHB - --- - CONFIG_STM32_DMA1 - CONFIG_STM32_DMA2 - CONFIG_STM32_CRC - CONFIG_STM32_FSMC - CONFIG_STM32_SDIO - - APB1 - ---- - CONFIG_STM32_TIM2 - CONFIG_STM32_TIM3 - CONFIG_STM32_TIM4 - CONFIG_STM32_TIM5 - CONFIG_STM32_TIM6 - CONFIG_STM32_TIM7 - CONFIG_STM32_WWDG - CONFIG_STM32_SPI2 - CONFIG_STM32_SPI4 - CONFIG_STM32_USART2 - CONFIG_STM32_USART3 - CONFIG_STM32_UART4 - CONFIG_STM32_UART5 - CONFIG_STM32_I2C1 - CONFIG_STM32_I2C2 - CONFIG_STM32_USB - CONFIG_STM32_CAN - CONFIG_STM32_BKP - CONFIG_STM32_PWR - CONFIG_STM32_DAC1 - CONFIG_STM32_DAC2 - CONFIG_STM32_USB - - APB2 - ---- - CONFIG_STM32_ADC1 - CONFIG_STM32_ADC2 - CONFIG_STM32_TIM1 - CONFIG_STM32_SPI1 - CONFIG_STM32_TIM8 - CONFIG_STM32_USART1 - CONFIG_STM32_ADC3 - - Timer and I2C devices may need to the following to force power to be applied - unconditionally at power up. (Otherwise, the device is powered when it is - initialized). - - CONFIG_STM32_FORCEPOWER - - Timer devices may be used for different purposes. One special purpose is - to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn - is defined (as above) then the following may also be defined to indicate that - the timer is intended to be used for pulsed output modulation, ADC conversion, - or DAC conversion. Note that ADC/DAC require two definition: Not only do you have - to assign the timer (n) for used by the ADC or DAC, but then you also have to - configure which ADC or DAC (m) it is assigned to. - - CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 - CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 - CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 - CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 - CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 - - For each timer that is enabled for PWM usage, we need the following additional - configuration settings: - - CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} - - NOTE: The STM32 timers are each capable of generating different signals on - each of the four channels with different duty cycles. That capability is - not supported by this driver: Only one output channel per timer. - - Alternate pin mappings (should not be used with the STM3210E-EVAL board): - - CONFIG_STM32_TIM1_FULL_REMAP - CONFIG_STM32_TIM1_PARTIAL_REMAP - CONFIG_STM32_TIM2_FULL_REMAP - CONFIG_STM32_TIM2_PARTIAL_REMAP_1 - CONFIG_STM32_TIM2_PARTIAL_REMAP_2 - CONFIG_STM32_TIM3_FULL_REMAP - CONFIG_STM32_TIM3_PARTIAL_REMAP - CONFIG_STM32_TIM4_REMAP - CONFIG_STM32_USART1_REMAP - CONFIG_STM32_USART2_REMAP - CONFIG_STM32_USART3_FULL_REMAP - CONFIG_STM32_USART3_PARTIAL_REMAP - CONFIG_STM32_SPI1_REMAP - CONFIG_STM32_SPI3_REMAP - CONFIG_STM32_I2C1_REMAP - CONFIG_STM32_CAN1_FULL_REMAP - CONFIG_STM32_CAN1_PARTIAL_REMAP - CONFIG_STM32_CAN2_REMAP - - JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): - CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - but without JNTRST. - CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled - - STM32F103Z specific device driver settings - - CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART - m (m=4,5) for the console and ttys0 (default is the USART1). - CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. - This specific the size of the receive buffer - CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before - being sent. This specific the size of the transmit buffer - CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be - CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. - CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity - CONFIG_U[S]ARTn_2STOP - Two stop bits - - CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI - support. Non-interrupt-driven, poll-waiting is recommended if the - interrupt rate would be to high in the interrupt driven case. - CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. - Cannot be used with CONFIG_STM32_SPI_INTERRUPT. - - CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO - and CONFIG_STM32_DMA2. - CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 - CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. - Default: Medium - CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: - 4-bit transfer mode. - - STM3210E-EVAL CAN Configuration - - CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or - CONFIG_STM32_CAN2 must also be defined) - CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default - Standard 11-bit IDs. - CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. - Default: 8 - CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. - Default: 4 - CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback - mode for testing. The STM32 CAN driver does support loopback mode. - CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. - CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. - CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 - CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 - CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an - dump of all CAN registers. - - STM3210E-EVAL LCD Hardware Configuration - - CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" - support. Default is this 320x240 "landscape" orientation - (this setting is informative only... not used). - CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" - orientation support. In this orientation, the STM3210E-EVAL's - LCD ribbon cable is at the bottom of the display. Default is - 320x240 "landscape" orientation. - CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse - portrait" orientation support. In this orientation, the - STM3210E-EVAL's LCD ribbon cable is at the top of the display. - Default is 320x240 "landscape" orientation. - CONFIG_LCD_BACKLIGHT - Define to support a backlight. - CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an - adjustable backlight will be provided using timer 1 to generate - various pulse widthes. The granularity of the settings is - determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or - CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight - is provided. - CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears - to be a shift in the returned data. This value fixes the offset. - Default 5. - - The LCD driver dynamically selects the LCD based on the reported LCD - ID value. However, code size can be reduced by suppressing support for - individual LCDs using: - - CONFIG_STM32_AM240320_DISABLE - CONFIG_STM32_SPFD5408B_DISABLE - CONFIG_STM32_R61580_DISABLE - -Configurations -============== - -Each STM3210E-EVAL configuration is maintained in a sudirectory and -can be selected as follow: - - cd tools - ./configure.sh stm3210e-eval/ - cd - - . ./setenv.sh - -Where is one of the following: - - buttons: - -------- - - Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and - button interrupts. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - - composite - --------- - - This configuration exercises a composite USB interface consisting - of a CDC/ACM device and a USB mass storage device. This configuration - uses apps/examples/composite. - - nsh and nsh2: - ------------ - Configure the NuttShell (nsh) located at examples/nsh. - - Differences between the two NSH configurations: - - =========== ======================= ================================ - nsh nsh2 - =========== ======================= ================================ - Toolchain: NuttX buildroot for Codesourcery for Windows (1) - Linux or Cygwin (1,2) - ----------- ----------------------- -------------------------------- - Loader: DfuSe DfuSe - ----------- ----------------------- -------------------------------- - Serial Debug output: USART1 Debug output: USART1 - Console: NSH output: USART1 NSH output: USART1 (3) - ----------- ----------------------- -------------------------------- - microSD Yes Yes - Support - ----------- ----------------------- -------------------------------- - FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y - Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) - ----------- ----------------------- -------------------------------- - Support for No Yes - Built-in - Apps - ----------- ----------------------- -------------------------------- - Built-in None apps/examples/nx - Apps apps/examples/nxhello - apps/examples/usbstorage (5) - =========== ======================= ================================ - - (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh - to set up the correct PATH variable for whichever toolchain you - may use. - (2) Since DfuSe is assumed, this configuration may only work under - Cygwin without modification. - (3) When any other device other than /dev/console is used for a user - interface, (1) linefeeds (\n) will not be expanded to carriage return - / linefeeds \r\n). You will need to configure your terminal program - to account for this. And (2) input is not automatically echoed so - you will have to turn local echo on. - (4) Microsoft holds several patents related to the design of - long file names in the FAT file system. Please refer to the - details in the top-level COPYING file. Please do not use FAT - long file name unless you are familiar with these patent issues. - (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), - Caution should be used to assure that the SD drive is not in use when - the USB storage device is configured. Specifically, the SD driver - should be unmounted like: - - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH - ... - nsh> umount /mnd/sdcard # Unmount before connecting USB!!! - nsh> msconn # Connect the USB storage device - ... - nsh> msdis # Disconnect USB storate device - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount - - Failure to do this could result in corruption of the SD card format. - - nx: - --- - An example using the NuttX graphics system (NX). This example - focuses on general window controls, movement, mouse and keyboard - input. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxlines: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing lines on the background in various - orientations. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxtext: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing text on the background while pop-up - windows occur. Text should continue to update normally with - or without the popup windows present. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - NOTE: When I tried building this example with the CodeSourcery - tools, I got a hardfault inside of its libgcc. I haven't - retested since then, but beware if you choose to change the - toolchain. - - ostest: - ------ - This configuration directory, performs a simple OS test using - examples/ostest. By default, this project assumes that you are - using the DFU bootloader. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - RIDE - ---- - This configuration builds a trivial bring-up binary. It is - useful only because it words with the RIDE7 IDE and R-Link debugger. - - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - - usbserial: - --------- - This configuration directory exercises the USB serial class - driver at examples/usbserial. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - USB debug output can be enabled as by changing the following - settings in the configuration file: - - -CONFIG_DEBUG=n - -CONFIG_DEBUG_VERBOSE=n - -CONFIG_DEBUG_USB=n - +CONFIG_DEBUG=y - +CONFIG_DEBUG_VERBOSE=y - +CONFIG_DEBUG_USB=y - - -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n - -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n - +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y - +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y - - By default, the usbserial example uses the Prolific PL2303 - serial/USB converter emulation. The example can be modified - to use the CDC/ACM serial class by making the following changes - to the configuration file: - - -CONFIG_PL2303=y - +CONFIG_PL2303=n - - -CONFIG_CDCACM=n - +CONFIG_CDCACM=y - - The example can also be converted to use the alternative - USB serial example at apps/examples/usbterm by changing the - following: - - -CONFIGURED_APPS += examples/usbserial - +CONFIGURED_APPS += examples/usbterm - - In either the original appconfig file (before configuring) - or in the final apps/.config file (after configuring). - - usbstorage: - ---------- - This configuration directory exercises the USB mass storage - class driver at examples/usbstorage. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - diff --git a/nuttx/configs/px4iov2/common/Make.defs b/nuttx/configs/px4iov2/common/Make.defs deleted file mode 100644 index 7f782b5b2..000000000 --- a/nuttx/configs/px4iov2/common/Make.defs +++ /dev/null @@ -1,175 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m3 \ - -mthumb \ - -march=armv7-m - -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx/configs/px4iov2/common/ld.script b/nuttx/configs/px4iov2/common/ld.script deleted file mode 100755 index 69c2f9cb2..000000000 --- a/nuttx/configs/px4iov2/common/ld.script +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * configs/stm3210e-eval/nsh/ld.script - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and - * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, - * FLASH memory is aliased to address 0x0000:0000 where the code expects to - * begin execution by jumping to the entry point in the 0x0800:0000 address - * range. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K -} - -OUTPUT_ARCH(arm) -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.glue_7) - *(.glue_7t) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx/configs/px4iov2/common/setenv.sh b/nuttx/configs/px4iov2/common/setenv.sh deleted file mode 100755 index ff9a4bf8a..000000000 --- a/nuttx/configs/px4iov2/common/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/include/README.txt b/nuttx/configs/px4iov2/include/README.txt deleted file mode 100755 index 2264a80aa..000000000 --- a/nuttx/configs/px4iov2/include/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains header files unique to the PX4IO board. diff --git a/nuttx/configs/px4iov2/include/board.h b/nuttx/configs/px4iov2/include/board.h deleted file mode 100755 index 21aacda87..000000000 --- a/nuttx/configs/px4iov2/include/board.h +++ /dev/null @@ -1,184 +0,0 @@ -/************************************************************************************ - * configs/px4io/include/board.h - * include/arch/board/board.h - * - * Copyright (C) 2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * 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 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. - * - ************************************************************************************/ - -#ifndef __ARCH_BOARD_BOARD_H -#define __ARCH_BOARD_BOARD_H - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include -#ifndef __ASSEMBLY__ -# include -# include -#endif -#include -#include -#include - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/* Clocking *************************************************************************/ - -/* On-board crystal frequency is 24MHz (HSE) */ - -#define STM32_BOARD_XTAL 24000000ul - -/* Use the HSE output as the system clock */ - -#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE -#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE -#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL - -/* AHB clock (HCLK) is SYSCLK (24MHz) */ - -#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK -#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY -#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ - -/* APB2 clock (PCLK2) is HCLK (24MHz) */ - -#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK -#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY -#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ - -/* APB2 timer 1 will receive PCLK2. */ - -#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) - -/* APB1 clock (PCLK1) is HCLK (24MHz) */ - -#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK -#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) - -/* All timers run off PCLK */ - -#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) -#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) -#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) - -/* - * Some of the USART pins are not available; override the GPIO - * definitions with an invalid pin configuration. - */ -#undef GPIO_USART2_CTS -#define GPIO_USART2_CTS 0xffffffff -#undef GPIO_USART2_RTS -#define GPIO_USART2_RTS 0xffffffff -#undef GPIO_USART2_CK -#define GPIO_USART2_CK 0xffffffff -#undef GPIO_USART3_TX -#define GPIO_USART3_TX 0xffffffff -#undef GPIO_USART3_CK -#define GPIO_USART3_CK 0xffffffff -#undef GPIO_USART3_CTS -#define GPIO_USART3_CTS 0xffffffff -#undef GPIO_USART3_RTS -#define GPIO_USART3_RTS 0xffffffff - -/* - * High-resolution timer - */ -#ifdef CONFIG_HRT_TIMER -# define HRT_TIMER 1 /* use timer1 for the HRT */ -# define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ -#endif - -/* - * PPM - * - * PPM input is handled by the HRT timer. - * - * Pin is PA8, timer 1, channel 1 - */ -#if defined(CONFIG_HRT_TIMER) && defined (CONFIG_HRT_PPM) -# define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -# define GPIO_PPM_IN GPIO_TIM1_CH1IN -#endif - -/* LED definitions ******************************************************************/ -/* PX4 has two LEDs that we will encode as: */ - -#define LED_STARTED 0 /* LED? */ -#define LED_HEAPALLOCATE 1 /* LED? */ -#define LED_IRQSENABLED 2 /* LED? + LED? */ -#define LED_STACKCREATED 3 /* LED? */ -#define LED_INIRQ 4 /* LED? + LED? */ -#define LED_SIGNAL 5 /* LED? + LED? */ -#define LED_ASSERTION 6 /* LED? + LED? + LED? */ -#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ - -/************************************************************************************ - * Public Data - ************************************************************************************/ - -#ifndef __ASSEMBLY__ - -#undef EXTERN -#if defined(__cplusplus) -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -/************************************************************************************ - * Public Function Prototypes - ************************************************************************************/ -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -EXTERN void stm32_boardinitialize(void); - -#if defined(__cplusplus) -} -#endif - -#endif /* __ASSEMBLY__ */ -#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4iov2/io/Make.defs b/nuttx/configs/px4iov2/io/Make.defs deleted file mode 100644 index 369772d03..000000000 --- a/nuttx/configs/px4iov2/io/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/px4io/common/Make.defs diff --git a/nuttx/configs/px4iov2/io/appconfig b/nuttx/configs/px4iov2/io/appconfig deleted file mode 100644 index 48a41bcdb..000000000 --- a/nuttx/configs/px4iov2/io/appconfig +++ /dev/null @@ -1,32 +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. -# -############################################################################ diff --git a/nuttx/configs/px4iov2/io/defconfig b/nuttx/configs/px4iov2/io/defconfig deleted file mode 100755 index 1eaf4f2d1..000000000 --- a/nuttx/configs/px4iov2/io/defconfig +++ /dev/null @@ -1,548 +0,0 @@ -############################################################################ -# configs/px4io/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD="px4iov2" -CONFIG_ARCH_BOARD_PX4IOV2=y -CONFIG_BOARD_LOOPSPERMSEC=2000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y -CONFIG_ARCH_MATH_H=y - -CONFIG_ARMV7M_CMNVECTOR=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n - -# -# Individual subsystems can be enabled: -# -# AHB: -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=n -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -# We use our own ADC driver, but leave this on for clocking purposes. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n - - -# -# STM32F100 specific serial device driver settings -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_SERIAL_TERMIOS=y -CONFIG_STANDARD_SERIAL=y - -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n - -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 - -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 - -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 - -CONFIG_USART1_RXDMA=y -SERIAL_HAVE_CONSOLE_DMA=y -# Conflicts with I2C1 DMA -CONFIG_USART2_RXDMA=n -CONFIG_USART3_RXDMA=y - -# -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API -# -CONFIG_USER_ENTRYPOINT="user_start" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_MSEC_PER_TICK=1 -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=8 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -# this eats ~1KiB of RAM ... work out why -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=y -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=50000 -CONFIG_SCHED_WORKSTACKSIZE=1024 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=n -CONFIG_SCHED_ATEXIT=n - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=y -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=12 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STDIO_LINEBUFFER=n -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=0 - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# - -# Disable NSH completely -CONFIG_NSH_CONSOLE=n - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1200 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=1024 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4iov2/io/setenv.sh b/nuttx/configs/px4iov2/io/setenv.sh deleted file mode 100755 index ff9a4bf8a..000000000 --- a/nuttx/configs/px4iov2/io/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/nsh/Make.defs b/nuttx/configs/px4iov2/nsh/Make.defs deleted file mode 100644 index 87508e22e..000000000 --- a/nuttx/configs/px4iov2/nsh/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs diff --git a/nuttx/configs/px4iov2/nsh/appconfig b/nuttx/configs/px4iov2/nsh/appconfig deleted file mode 100644 index 3cfc41b43..000000000 --- a/nuttx/configs/px4iov2/nsh/appconfig +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# configs/stm3210e-eval/nsh/appconfig -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# Path to example in apps/examples containing the user_start entry point - -CONFIGURED_APPS += examples/nsh - -CONFIGURED_APPS += system/readline -CONFIGURED_APPS += nshlib -CONFIGURED_APPS += reboot - -CONFIGURED_APPS += drivers/boards/px4iov2 diff --git a/nuttx/configs/px4iov2/nsh/defconfig b/nuttx/configs/px4iov2/nsh/defconfig deleted file mode 100755 index 14d7a6401..000000000 --- a/nuttx/configs/px4iov2/nsh/defconfig +++ /dev/null @@ -1,567 +0,0 @@ -############################################################################ -# configs/px4io/nsh/defconfig -# -# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip familyl. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_DRAM_END - Last address+1 of installed RAM -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH=arm -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP=stm32 -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD=px4iov2 -CONFIG_ARCH_BOARD_PX4IOV2=y -CONFIG_BOARD_LOOPSPERMSEC=24000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=y -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=n -CONFIG_ARMV7M_CMNVECTOR=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n - -# -# Individual subsystems can be enabled: -# AHB: -CONFIG_STM32_DMA1=n -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n - -# -# Timer and I2C devices may need to the following to force power to be applied: -# -#CONFIG_STM32_FORCEPOWER=y - -# -# STM32F100 specific serial device driver settings -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n - -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 - -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 - -CONFIG_USART1_BAUD=57600 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 - -# -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y -CONFIG_PWM_SERVO=y - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_NXFLAT. Enable support for the NXFLAT binary format. -# This format will support execution of NuttX binaries located -# in a ROMFS filesystem (see examples/nxflat). -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=n -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=200 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=0 -CONFIG_START_YEAR=2009 -CONFIG_START_MONTH=9 -CONFIG_START_DAY=21 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=n -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_NXFLAT=n -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=(50*1000) -CONFIG_SCHED_WORKSTACKSIZE=512 -CONFIG_SIG_SIGWORK=4 - -CONFIG_USER_ENTRYPOINT="nsh_main" -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=n -CONFIG_DISABLE_SIGNALS=n -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=6 -CONFIG_NFILE_STREAMS=4 -CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=64 -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=1 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=3 -CONFIG_PREALLOC_TIMERS=1 - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# -# If CONFIG_NSH_TELNET is selected: -# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size -# CONFIG_NSH_DHCPC - Obtain address using DHCP -# CONFIG_NSH_IPADDR - Provides static IP address -# CONFIG_NSH_DRIPADDR - Provides static router IP address -# CONFIG_NSH_NETMASK - Provides static network mask -# CONFIG_NSH_NOMAC - Use a bogus MAC address -# -# If CONFIG_NSH_ROMFSETC is selected: -# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint -# CONFIG_NSH_INITSCRIPT - Relative path to init script -# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor -# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size -# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor -# CONFIG_NSH_FATSECTSIZE - FAT FS sector size -# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors -# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint -# -CONFIG_BUILTIN=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_FILEIOSIZE=64 -CONFIG_NSH_STRERROR=n -CONFIG_NSH_LINELEN=64 -CONFIG_NSH_NESTDEPTH=1 -CONFIG_NSH_DISABLESCRIPT=y -CONFIG_NSH_DISABLEBG=n -CONFIG_NSH_ROMFSETC=n -CONFIG_NSH_CONSOLE=y -CONFIG_NSH_TELNET=n -CONFIG_NSH_ARCHINIT=n -CONFIG_NSH_IOBUFFER_SIZE=256 -CONFIG_NSH_STACKSIZE=1024 -CONFIG_NSH_DHCPC=n -CONFIG_NSH_NOMAC=n -CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) -CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) -CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0) -CONFIG_NSH_ROMFSMOUNTPT="/etc" -CONFIG_NSH_INITSCRIPT="init.d/rcS" -CONFIG_NSH_ROMFSDEVNO=0 -CONFIG_NSH_ROMFSSECTSIZE=64 -CONFIG_NSH_FATDEVNO=1 -CONFIG_NSH_FATSECTSIZE=512 -CONFIG_NSH_FATNSECTORS=1024 -CONFIG_NSH_FATMOUNTPT=/tmp - -# -# Architecture-specific NSH options -# -CONFIG_NSH_MMCSDSPIPORTNO=0 -CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDMINOR=0 - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=800 -CONFIG_USERMAIN_STACKSIZE=1024 -CONFIG_PTHREAD_STACK_MIN=256 -CONFIG_PTHREAD_STACK_DEFAULT=512 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4iov2/nsh/setenv.sh b/nuttx/configs/px4iov2/nsh/setenv.sh deleted file mode 100755 index d83685192..000000000 --- a/nuttx/configs/px4iov2/nsh/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4iov2/src/Makefile b/nuttx/configs/px4iov2/src/Makefile deleted file mode 100644 index bb9539d16..000000000 --- a/nuttx/configs/px4iov2/src/Makefile +++ /dev/null @@ -1,84 +0,0 @@ -############################################################################ -# configs/stm3210e-eval/src/Makefile -# -# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - --include $(TOPDIR)/Make.defs - -CFLAGS += -I$(TOPDIR)/sched - -ASRCS = -AOBJS = $(ASRCS:.S=$(OBJEXT)) - -CSRCS = empty.c - -COBJS = $(CSRCS:.c=$(OBJEXT)) - -SRCS = $(ASRCS) $(CSRCS) -OBJS = $(AOBJS) $(COBJS) - -ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src -ifeq ($(WINTOOL),y) - CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ - -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ - -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" -else - CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m -endif - -all: libboard$(LIBEXT) - -$(AOBJS): %$(OBJEXT): %.S - $(call ASSEMBLE, $<, $@) - -$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c - $(call COMPILE, $<, $@) - -libboard$(LIBEXT): $(OBJS) - $(call ARCHIVE, $@, $(OBJS)) - -.depend: Makefile $(SRCS) - @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ - -depend: .depend - -clean: - $(call DELFILE, libboard$(LIBEXT)) - $(call CLEAN) - -distclean: clean - $(call DELFILE, Make.dep) - $(call DELFILE, .depend) - --include Make.dep diff --git a/nuttx/configs/px4iov2/src/README.txt b/nuttx/configs/px4iov2/src/README.txt deleted file mode 100644 index d4eda82fd..000000000 --- a/nuttx/configs/px4iov2/src/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx/configs/px4iov2/src/empty.c b/nuttx/configs/px4iov2/src/empty.c deleted file mode 100644 index ace900866..000000000 --- a/nuttx/configs/px4iov2/src/empty.c +++ /dev/null @@ -1,4 +0,0 @@ -/* - * There are no source files here, but libboard.a can't be empty, so - * we have this empty source file to keep it company. - */ diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 03ec5a255..f9d1b8022 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -59,9 +59,9 @@ #include #include -#include "stm32_internal.h" +#include #include "px4fmu_internal.h" -#include "stm32_uart.h" +#include #include diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index dd291b9b7..dce51bcda 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -50,7 +50,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ -#include +#include /**************************************************************************************************** diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c index 14e4052be..d1656005b 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c index f90f25071..5e90c227d 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_spi.c @@ -50,9 +50,9 @@ #include #include -#include "up_arch.h" -#include "chip.h" -#include "stm32_internal.h" +#include +#include +#include #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c index b0b669fbe..3492e2c68 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_usb.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_usb.c @@ -51,8 +51,8 @@ #include #include -#include "up_arch.h" -#include "stm32_internal.h" +#include +#include #include "px4fmu_internal.h" /************************************************************************************ diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c index 5d7b22560..e95298bf5 100644 --- a/src/drivers/boards/px4iov2/px4iov2_init.c +++ b/src/drivers/boards/px4iov2/px4iov2_init.c @@ -54,7 +54,7 @@ #include -#include "stm32_internal.h" +#include #include "px4iov2_internal.h" #include diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h index 81a75431c..2bf65e047 100755 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ b/src/drivers/boards/px4iov2/px4iov2_internal.h @@ -48,7 +48,7 @@ #include /* these headers are not C++ safe */ -#include +#include /****************************************************************************** diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c index 5e1aafa49..4f1b9487c 100644 --- a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c +++ b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index a4c59d218..faeb9cf60 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -42,7 +42,7 @@ #include -#ifdef CONFIG_ARCH_BOARD_PX4FMU +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* * PX4FMU GPIO numbers. * @@ -67,7 +67,7 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 /* * PX4FMUv2 GPIO numbers. * @@ -93,36 +93,14 @@ #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO -/* - * PX4IO GPIO numbers. - * - * XXX note that these are here for reference/future use; currently - * there is no good way to wire these up without a common STM32 GPIO - * driver, which isn't implemented yet. - */ -/* outputs */ -# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ -# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ -# define GPIO_SERVO_POWER (1<<2) /**< servo power */ -# define GPIO_RELAY1 (1<<3) /**< relay 1 */ -# define GPIO_RELAY2 (1<<4) /**< relay 2 */ -# define GPIO_LED_BLUE (1<<5) /**< blue LED */ -# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ -# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ - -/* inputs */ -# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ -# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ -# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -# define GPIO_DEVICE_PATH "/dev/px4io" +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 +/* no GPIO driver on the PX4IOv1 board */ +# define GPIO_DEVICE_PATH "/nonexistent" +#endif +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +/* no GPIO driver on the PX4IOv2 board */ +# define GPIO_DEVICE_PATH "/nonexistent" #endif #ifndef GPIO_DEVICE_PATH diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index ff99de02f..7d3af4b0d 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -59,10 +59,10 @@ #include #include -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) # include # define FMU_HAVE_PPM -#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) # include # undef FMU_HAVE_PPM #else @@ -158,7 +158,7 @@ private: }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -168,7 +168,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, @@ -873,7 +873,7 @@ PX4FMU::gpio_reset(void) } } -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* if we have a GPIO direction control, set it to zero (input) */ stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_configgpio(GPIO_GPIO_DIR); @@ -883,7 +883,7 @@ PX4FMU::gpio_reset(void) void PX4FMU::gpio_set_function(uint32_t gpios, int function) { -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -918,7 +918,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } } -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); @@ -1024,17 +1024,17 @@ fmu_new_mode(PortMode new_mode) break; case PORT_FULL_PWM: -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; #endif -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) servo_mode = PX4FMU::MODE_6PWM; #endif break; /* mixed modes supported on v1 board only */ -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) case PORT_FULL_SERIAL: /* set all multi-GPIOs to serial mode */ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; @@ -1116,7 +1116,7 @@ test(void) if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); @@ -1183,7 +1183,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) } else if (!strcmp(verb, "mode_serial")) { new_mode = PORT_FULL_SERIAL; @@ -1217,9 +1217,9 @@ fmu_main(int argc, char *argv[]) fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); -#if defined(CONFIG_ARCH_BOARD_PX4FMU) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) fprintf(stderr, " mode_gpio, mode_pwm\n"); #endif exit(1); diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp index 9727daa71..06b955971 100644 --- a/src/drivers/px4io/interface_serial.cpp +++ b/src/drivers/px4io/interface_serial.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 15e213afb..29624018f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1604,9 +1604,9 @@ start(int argc, char *argv[]) PX4IO_interface *interface; -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) interface = io_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); #else # error Unknown board - cannot select interface. @@ -1741,9 +1741,9 @@ if_test(unsigned mode) { PX4IO_interface *interface; -#if defined(CONFIG_ARCH_BOARD_PX4FMUV2) +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) interface = io_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); #else # error Unknown board - cannot select interface. diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp index 28c584438..ec22a5e17 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/uploader.cpp @@ -56,10 +56,10 @@ #include "uploader.h" -#ifdef CONFIG_ARCH_BOARD_PX4FMUV2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #include #endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #include #endif diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 7ef3db970..1cc18afda 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -70,7 +70,7 @@ #include "stm32_gpio.h" #include "stm32_tim.h" -#ifdef CONFIG_HRT_TIMER +#ifdef HRT_TIMER /* HRT configuration */ #if HRT_TIMER == 1 @@ -275,7 +275,7 @@ static void hrt_call_invoke(void); /* * Specific registers and bits used by PPM sub-functions */ -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * @@ -377,7 +377,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCMR1_PPM 0 # define CCMR2_PPM 0 # define CCER_PPM 0 -#endif /* CONFIG_HRT_PPM */ +#endif /* HRT_PPM_CHANNEL */ /* * Initialise the timer we are going to use. @@ -907,4 +907,4 @@ hrt_latency_update(void) } -#endif /* CONFIG_HRT_TIMER */ +#endif /* HRT_TIMER */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 167ef30a8..2284be84d 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -117,10 +117,6 @@ #include -#ifndef CONFIG_HRT_TIMER -# error This driver requires CONFIG_HRT_TIMER -#endif - /* Tone alarm configuration */ #if TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 4dd1aa8d7..59f470a94 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -15,10 +15,10 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -ifeq ($(BOARD),px4io) +ifeq ($(BOARD),px4io-v1) SRCS += i2c.c endif -ifeq ($(BOARD),px4iov2) +ifeq ($(BOARD),px4io-v2) SRCS += serial.c \ ../systemlib/hx_stream.c endif diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b32782285..ccf175e45 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,10 +42,10 @@ #include #include -#ifdef CONFIG_ARCH_BOARD_PX4IO +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 # include #endif -#ifdef CONFIG_ARCH_BOARD_PX4IOV2 +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 # include #endif @@ -129,18 +129,7 @@ extern struct sys_state_s system_state; #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#ifdef CONFIG_ARCH_BOARD_PX4IOV2 - -# define PX4IO_RELAY_CHANNELS 0 -# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) - -# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) - -# define PX4IO_ADC_CHANNEL_COUNT 2 -# define ADC_VSERVO 4 -# define ADC_RSSI 5 - -#else /* CONFIG_ARCH_BOARD_PX4IOV1 */ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 # define PX4IO_RELAY_CHANNELS 4 # define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) @@ -158,6 +147,19 @@ extern struct sys_state_s system_state; #endif +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + +#endif + #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f4541936b..873ee73f1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -59,7 +59,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt */ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, -#ifdef CONFIG_ARCH_BOARD_PX4IOV2 +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, #else [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e4bc68f58..2f3184623 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include //#define DEBUG -- cgit v1.2.3 From 0efb1d6cebbbe1889aceb12329a97a1c85126cce Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 18:50:04 -0700 Subject: Fix the px4io serial port device name now that we're not using UART8 as the console. --- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index dce51bcda..76aa042f6 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -59,7 +59,7 @@ __BEGIN_DECLS /* Configuration ************************************************************************************/ /* PX4IO connection configuration */ -#define PX4IO_SERIAL_DEVICE "/dev/ttyS5" +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" #define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX #define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX #define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ -- cgit v1.2.3 From e2458677c99f7b74462381a3cd9dec3321901190 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 7 Jul 2013 20:42:03 -0700 Subject: Tweak IO serial packet error handling slightly; on reception of a serial error send a line break back to FMU. This should cause FMU to stop sending immediately. Flag these cases and discard the packet rather than processing it, rather than simply dropping the received packet and letting FMU time out. --- src/modules/px4iofirmware/serial.c | 44 +++++++++++++++++++++++++++++--------- 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 2f3184623..94d7407df 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -221,6 +221,10 @@ rx_handle_packet(void) static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ perf_begin(pc_txns); /* disable UART DMA */ @@ -235,7 +239,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* re-set DMA for reception first, so we are ready to receive before we start sending */ dma_reset(); - /* send the reply to the previous request */ + /* send the reply to the just-processed request */ dma_packet.crc = 0; dma_packet.crc = crc_packet(&dma_packet); stm32_dmasetup( @@ -256,6 +260,8 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) static int serial_interrupt(int irq, void *context) { + static bool abort_on_idle = false; + uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* required to clear any of the interrupt status that brought us here */ @@ -271,28 +277,46 @@ serial_interrupt(int irq, void *context) if (sr & USART_SR_FE) perf_count(pc_fe); - /* reset DMA state machine back to listening-for-packet */ - dma_reset(); + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; - /* don't attempt to handle IDLE if it's set - things went bad */ - return 0; + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; } if (sr & USART_SR_IDLE) { - /* the packet might have been short - go check */ + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ perf_count(pc_badidle); return 0; } + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ perf_count(pc_idle); - - /* stop the receive DMA */ stm32_dmastop(rx_dma); - - /* and treat this like DMA completion */ rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); } -- cgit v1.2.3 From 04fbed493aab1dede6c2200aaab2b990d24a66e7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 9 Jul 2013 14:09:48 +0400 Subject: multirotor_pos_control: use separate PID controllers for position and velocity --- src/modules/multirotor_pos_control/module.mk | 3 +- .../multirotor_pos_control.c | 193 +++++++++++---------- .../multirotor_pos_control_params.c | 63 ++++--- .../multirotor_pos_control_params.h | 42 +++-- src/modules/multirotor_pos_control/thrust_pid.c | 179 +++++++++++++++++++ src/modules/multirotor_pos_control/thrust_pid.h | 75 ++++++++ .../position_estimator_inav_main.c | 2 +- 7 files changed, 421 insertions(+), 136 deletions(-) create mode 100644 src/modules/multirotor_pos_control/thrust_pid.c create mode 100644 src/modules/multirotor_pos_control/thrust_pid.h diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d04847745..bc4b48fb4 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index acae03fae..d56c3d58f 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,9 +61,11 @@ #include #include #include +#include #include #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -84,8 +86,6 @@ static void usage(const char *reason); static float scale_control(float ctl, float end, float dz); -static float limit_value(float v, float limit); - static float norm(float x, float y); static void usage(const char *reason) { @@ -110,11 +110,12 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor_pos_control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, @@ -126,15 +127,16 @@ int multirotor_pos_control_main(int argc, char *argv[]) { } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor_pos_control app is running\n"); + warnx("app is running"); } else { - printf("\tmultirotor_pos_control app not started\n"); + warnx("app not started"); } exit(0); } @@ -153,22 +155,13 @@ static float scale_control(float ctl, float end, float dz) { } } -static float limit_value(float v, float limit) { - if (v > limit) { - v = limit; - } else if (v < -limit) { - v = -limit; - } - return v; -} - static float norm(float x, float y) { return sqrtf(x * x + y * y); } static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - warnx("started."); + warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -203,15 +196,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_alt = true; bool reset_sp_pos = true; hrt_abstime t_prev = 0; - float alt_integral = 0.0f; /* integrate in NED frame to estimate wind but not attitude offset */ - float pos_x_integral = 0.0f; - float pos_y_integral = 0.0f; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + static PID_t xy_pos_pids[2]; + static PID_t xy_vel_pids[2]; + static PID_t z_pos_pid; + static thrust_pid_t z_vel_pid; + thread_running = true; struct multirotor_position_control_params params; @@ -219,6 +214,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + } + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + int paramcheck_counter = 0; while (!thread_should_exit) { @@ -231,6 +233,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_check(param_sub, ¶m_updated); if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + } + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } paramcheck_counter = 0; } @@ -269,7 +277,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; + z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -277,18 +285,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - pos_x_integral = 0.0f; - pos_y_integral = 0.0f; + xy_vel_pids[0].integral = 0.0f; + xy_vel_pids[1].integral = 0.0f; mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; + float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float pos_sp_speed_x = 0.0f; - float pos_sp_speed_y = 0.0f; - float pos_sp_speed_z = 0.0f; + float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + /* manual control */ if (status.flag_control_manual_enabled) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { @@ -299,14 +306,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { home_alt = local_pos.home_alt; } /* move altitude setpoint with manual controls */ - float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - if (alt_sp_ctl != 0.0f) { - pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; - local_pos_sp.z += pos_sp_speed_z * dt; - if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { - local_pos_sp.z = local_pos.z + alt_err_linear_limit; - } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { - local_pos_sp.z = local_pos.z - alt_err_linear_limit; + float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { + sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; + local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { + local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { + local_pos_sp.z = local_pos.z - z_sp_offs_max; } } @@ -316,76 +323,84 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - local_pos_sp.x += pos_sp_speed_x * dt; - local_pos_sp.y += pos_sp_speed_y * dt; + float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; + sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + local_pos_sp.x += sp_move_rate[0] * dt; + local_pos_sp.y += sp_move_rate[1] * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; } } } + } - if (params.hard == 0) { - pos_sp_speed_x = 0.0f; - pos_sp_speed_y = 0.0f; - pos_sp_speed_z = 0.0f; - } + /* run position & altitude controllers, calculate velocity setpoint */ + float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; + vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate velocity set point in NED frame */ + vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + } else { + reset_sp_pos = true; + } + /* calculate direction and norm of thrust in NED frame + * limit 3D speed by ellipsoid: + * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ + float v; + float vel_sp_norm = 0.0f; + v = vel_sp[0] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[1] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[2] / params.z_vel_max; + vel_sp_norm += v * v; + vel_sp_norm = sqrtf(vel_sp_norm); + if (vel_sp_norm > 1.0f) { + vel_sp[0] /= vel_sp_norm; + vel_sp[1] /= vel_sp_norm; + vel_sp[2] /= vel_sp_norm; } - /* PID for altitude */ - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); - /* P and D components */ - float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z - /* integrate */ - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - if (alt_integral < params.thr_min) { - alt_integral = params.thr_min; - } else if (alt_integral > params.thr_max) { - alt_integral = params.thr_max; + /* run velocity controllers, calculate thrust vector */ + float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate velocity set point in NED frame */ + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); } - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ + /* limit horizontal part of thrust */ + float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); + float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { + thrust_xy_norm = params.slope_max; } + /* use approximation: slope ~ sin(slope) = force */ + /* convert direction to body frame */ + thrust_xy_dir -= att.yaw; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - /* PID for position */ - /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ - float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); - float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); - /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; - /* integrate */ - pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); - pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; - /* calculate direction and slope in NED frame */ - float dir = atan2f(pos_y_ctl, pos_x_ctl); - /* use approximation: slope ~ sin(slope) = force */ - float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); - /* convert direction to body frame */ - dir -= att.yaw; /* calculate roll and pitch */ - att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch - att_sp.roll_body = sinf(dir) * slope; - } else { - reset_sp_pos = true; + att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; + att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch } - att_sp.thrust = thrust_ctl; + /* attitude-thrust compensation */ + float att_comp; + if (att.R[2][2] > 0.8f) + att_comp = 1.0f / att.R[2][2]; + else if (att.R[2][2] > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; + else + att_comp = 1.0f; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ @@ -403,8 +418,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } - printf("[multirotor_pos_control] exiting\n"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); thread_running = false; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index d284de433..f8a982c6c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -44,31 +44,37 @@ /* controller parameters */ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); -PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); -PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); -PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - h->alt_p = param_find("MPC_ALT_P"); - h->alt_i = param_find("MPC_ALT_I"); - h->alt_d = param_find("MPC_ALT_D"); - h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); - h->pos_p = param_find("MPC_POS_P"); - h->pos_i = param_find("MPC_POS_I"); - h->pos_d = param_find("MPC_POS_D"); - h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); + h->z_p = param_find("MPC_Z_P"); + h->z_d = param_find("MPC_Z_D"); + h->z_vel_p = param_find("MPC_Z_VEL_P"); + h->z_vel_i = param_find("MPC_Z_VEL_I"); + h->z_vel_d = param_find("MPC_Z_VEL_D"); + h->z_vel_max = param_find("MPC_Z_VEL_MAX"); + h->xy_p = param_find("MPC_XY_P"); + h->xy_d = param_find("MPC_XY_D"); + h->xy_vel_p = param_find("MPC_XY_VEL_P"); + h->xy_vel_i = param_find("MPC_XY_VEL_I"); + h->xy_vel_d = param_find("MPC_XY_VEL_D"); + h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); - h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -81,16 +87,19 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, { param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); - param_get(h->alt_p, &(p->alt_p)); - param_get(h->alt_i, &(p->alt_i)); - param_get(h->alt_d, &(p->alt_d)); - param_get(h->alt_rate_max, &(p->alt_rate_max)); - param_get(h->pos_p, &(p->pos_p)); - param_get(h->pos_i, &(p->pos_i)); - param_get(h->pos_d, &(p->pos_d)); - param_get(h->pos_rate_max, &(p->pos_rate_max)); + param_get(h->z_p, &(p->z_p)); + param_get(h->z_d, &(p->z_d)); + param_get(h->z_vel_p, &(p->z_vel_p)); + param_get(h->z_vel_i, &(p->z_vel_i)); + param_get(h->z_vel_d, &(p->z_vel_d)); + param_get(h->z_vel_max, &(p->z_vel_max)); + param_get(h->xy_p, &(p->xy_p)); + param_get(h->xy_d, &(p->xy_d)); + param_get(h->xy_vel_p, &(p->xy_vel_p)); + param_get(h->xy_vel_i, &(p->xy_vel_i)); + param_get(h->xy_vel_d, &(p->xy_vel_d)); + param_get(h->xy_vel_max, &(p->xy_vel_max)); param_get(h->slope_max, &(p->slope_max)); - param_get(h->hard, &(p->hard)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 13b667ad3..e9b1f5c39 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -44,16 +44,19 @@ struct multirotor_position_control_params { float thr_min; float thr_max; - float alt_p; - float alt_i; - float alt_d; - float alt_rate_max; - float pos_p; - float pos_i; - float pos_d; - float pos_rate_max; + float z_p; + float z_d; + float z_vel_p; + float z_vel_i; + float z_vel_d; + float z_vel_max; + float xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; float slope_max; - int hard; float rc_scale_pitch; float rc_scale_roll; @@ -63,16 +66,19 @@ struct multirotor_position_control_params { struct multirotor_position_control_param_handles { param_t thr_min; param_t thr_max; - param_t alt_p; - param_t alt_i; - param_t alt_d; - param_t alt_rate_max; - param_t pos_p; - param_t pos_i; - param_t pos_d; - param_t pos_rate_max; + param_t z_p; + param_t z_d; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t xy_p; + param_t xy_d; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; param_t slope_max; - param_t hard; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 000000000..89efe1334 --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Anton Babushkin + * Julian Oes + * + * 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 thrust_pid.c + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Anton Babushkin + * @author Julian Oes + */ + +#include "thrust_pid.h" +#include + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +{ + /* Alternative integral component calculation + error = setpoint - actual_position + integral = integral + (Ki*error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + integral + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (pid->ki * error * dt); + + float output = (error * pid->kp) + i + (d * pid->kd); + if (output < pid->limit_min || output > pid->limit_max) { + i = pid->integral; // If saturated then do not update integral value + // recalculate output with old integral + output = (error * pid->kp) + i + (d * pid->kd); + + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h new file mode 100644 index 000000000..65ee33c51 --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 thrust_pid.h + * + * Definition of thrust control PID interface. + * + * @author Anton Babushkin + */ + +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ + +#include + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); + +__END_DECLS + +#endif /* THRUST_PID_H_ */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fb5a779bc..9db2633c6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -120,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = true; thread_should_exit = false; - position_estimator_inav_task = task_spawn("position_estimator_inav", + position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); -- cgit v1.2.3 From 43eca3eb0e0267f1eb314fc2f5ee6d9eaef93d80 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Jun 2013 12:39:33 +1000 Subject: build: use unqualified com port names on windows the qualified names were not working with current versions of python --- makefiles/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 4b01b447d..a0671f6ad 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -18,7 +18,7 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" endif .PHONY: all upload-$(METHOD)-$(BOARD) -- cgit v1.2.3 From f5b91e109df755a6171264b59e92099b3ab20dbe Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 10 Jul 2013 23:50:37 -0700 Subject: More GPIO and general pin assignment fixes. --- src/drivers/drv_gpio.h | 9 ++++++--- src/drivers/px4fmu/fmu.cpp | 22 +++++++++++++--------- src/drivers/stm32/adc/adc.cpp | 12 +++++++++--- 3 files changed, 28 insertions(+), 15 deletions(-) diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index faeb9cf60..5b06f91a8 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -80,9 +80,12 @@ # define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ # define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ -# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - VDD_5V_PERIPH_EN */ -# define GPIO_5V_HIPOWER_OC (1<<7) /**< PE10 - !VDD_5V_HIPOWER_OC */ -# define GPIO_5V_PERIPH_OC (1<<8) /**< PE15 - !VDD_5V_PERIPH_OC */ +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - !VDD_5V_PERIPH_EN */ +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ +# define GPIO_SERVO_VALID (1<<9) /**< PB7 - !VDD_SERVO_VALID */ +# define GPIO_5V_HIPOWER_OC (1<<10) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */ /** * Default GPIO device - other devices may also support this protocol if diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7d3af4b0d..7928752d5 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -169,15 +169,19 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, - {0, GPIO_VDD_5V_PERIPH_EN, 0}, - {GPIO_5V_HIPOWER_OC, 0, 0}, - {GPIO_5V_PERIPH_OC, 0, 0}, + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, + {GPIO_VDD_SERVO_VALID, 0, 0}, + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif }; diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 1020eb946..48c95b3dd 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -227,7 +227,6 @@ ADC::init() if ((hrt_absolute_time() - now) > 500) { log("sample timeout"); return -1; - return 0xffff; } } @@ -282,7 +281,7 @@ ADC::close_last(struct file *filp) void ADC::_tick_trampoline(void *arg) { - ((ADC *)arg)->_tick(); + (reinterpret_cast(arg))->_tick(); } void @@ -366,8 +365,15 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */ g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + /* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */ + g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | + (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); -- cgit v1.2.3 From c51c211bbaef70defe73c91f6acd6ab99f0f3a04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 08:45:49 +0400 Subject: position_estimator_inav: global position altitude fixed --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 9db2633c6..3b0fbd782 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -548,7 +548,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = -local_pos.z - local_pos.home_alt; + global_pos.alt = local_pos.home_alt - local_pos.z; global_pos.relative_alt = -local_pos.z; global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; -- cgit v1.2.3 From 16cb0a793d60e55efe0b851ff52f31a4c7770834 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Jul 2013 23:23:10 -0700 Subject: Some more v2 pin / gpio configs missed in the previous commit --- src/drivers/boards/px4fmuv2/px4fmu_init.c | 20 ++++++++++++++------ src/drivers/boards/px4fmuv2/px4fmu_internal.h | 4 +++- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index f9d1b8022..57f2812b8 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -149,13 +149,21 @@ __EXPORT int nsh_archinitialize(void) { /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - stm32_configgpio(GPIO_ADC1_IN12); - - /* configure power supply control pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h index 76aa042f6..ad66ce563 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -102,7 +102,9 @@ __BEGIN_DECLS /* Power supply control and monitoring GPIOs */ #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) -#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) #define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) -- cgit v1.2.3 From 95f59f521d7c18bf5348a23addb0471ce81f016e Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Jul 2013 23:23:58 -0700 Subject: Add builtin command dependency on module makefiles, since they can also create / remove commands --- makefiles/firmware.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f1c1b496a..3ad13088b 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -385,7 +385,7 @@ define BUILTIN_DEF endef # Don't generate until modules have updated their command files -$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES) @$(ECHO) "CMDS: $@" $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include ' >> $@ -- cgit v1.2.3 From c459901f263b19d13466c47fcb8e2dce828ab59c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Jul 2013 23:52:36 -0700 Subject: Let's have some direct-access I/O methods as well. --- src/drivers/device/cdev.cpp | 35 ++++++++++++++++++++++++++------- src/drivers/device/device.h | 47 ++++++++++++++++++++++++++++++++++++++++----- 2 files changed, 70 insertions(+), 12 deletions(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 422321850..dcbac25e3 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -111,21 +111,21 @@ CDev::~CDev() int CDev::init() { - int ret = OK; - // base class init first - ret = Device::init(); + int ret = Device::init(); if (ret != OK) goto out; // now register the driver - ret = register_driver(_devname, &fops, 0666, (void *)this); + if (_devname != nullptr) { + ret = register_driver(_devname, &fops, 0666, (void *)this); - if (ret != OK) - goto out; + if (ret != OK) + goto out; - _registered = true; + _registered = true; + } out: return ret; @@ -395,4 +395,25 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup) return cdev->poll(filp, fds, setup); } +int +CDev::read(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +CDev::write(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +CDev::ioctl(unsigned operation, unsigned &arg) +{ + errno = ENODEV; + return -1; +} + } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 7d375aab9..2cac86636 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -85,7 +85,7 @@ protected: */ Device(const char *name, int irq = 0); - ~Device(); + virtual ~Device(); /** * Initialise the driver and make it ready for use. @@ -189,7 +189,7 @@ public: /** * Destructor */ - ~CDev(); + virtual ~CDev(); virtual int init(); @@ -287,6 +287,43 @@ public: */ bool is_open() { return _open_count > 0; } + /* + * Direct access methods. + */ + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device offset at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int read(unsigned offset, void *data, unsigned count = 1); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of registers to write, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int write(unsigned address, void *data, unsigned count = 1); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform + * @param arg An argument to the operation. + * @return < 0 on error + */ + virtual int ioctl(unsigned operation, unsigned &arg); + protected: /** * Pointer to the default cdev file operations table; useful for @@ -396,9 +433,9 @@ public: const char *devname, uint32_t base, int irq = 0); - ~PIO(); + virtual ~PIO(); - int init(); + virtual int init(); protected: @@ -407,7 +444,7 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) { return *(volatile uint32_t *)(_base + offset); } -- cgit v1.2.3 From d4c6ebde338c8aa1b0d2c9574d3bbdeead525cea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 13:17:42 +0400 Subject: multirotor_pos_control: global_position_setpoint support in AUTO mode --- .../multirotor_pos_control.c | 39 +++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index d56c3d58f..3e5857c26 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -179,6 +180,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_position_setpoint_s global_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -188,6 +191,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); @@ -201,6 +205,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + uint64_t local_home_timestamp = 0; static PID_t xy_pos_pids[2]; static PID_t xy_vel_pids[2]; @@ -222,6 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; + bool global_pos_sp_updated = false; while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_status), state_sub, &status); @@ -243,6 +249,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } + /* only check global position setpoint updates but not read */ + if (!global_pos_sp_updated) { + orb_check(global_pos_sp_sub, &global_pos_sp_updated); + } + /* Check if controller should act */ bool act = status.flag_system_armed && ( /* SAS modes */ @@ -271,7 +282,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); if (status.state_machine == SYSTEM_STATE_AUTO) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + if (local_pos.home_timestamp != local_home_timestamp) { + local_home_timestamp = local_pos.home_timestamp; + /* init local projection using local position home */ + double lat_home = local_pos.home_lat * 1e-7; + double lon_home = local_pos.home_lon * 1e-7; + map_projection_init(lat_home, lon_home); + warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); + mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); + } + if (global_pos_sp_updated) { + global_pos_sp_updated = false; + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + } else { + local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + } + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } if (reset_sp_alt) { -- cgit v1.2.3 From 5937c3a31b39d2fe5b70c2eef6327562208f1042 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 16:30:11 +0400 Subject: uORB topic vehicle_global_velocity_setpoint added --- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vehicle_global_velocity_setpoint.h | 64 ++++++++++++++++++++++ 2 files changed, 67 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_global_velocity_setpoint.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ae5fc6c61..1d5f17405 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -110,6 +110,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp #include "topics/vehicle_global_position_set_triplet.h" ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/vehicle_global_velocity_setpoint.h" +ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); + #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h new file mode 100644 index 000000000..73961cdfe --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 vehicle_global_velocity_setpoint.h + * Definition of the global velocity setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_global_velocity_setpoint_s +{ + float vx; /**< in m/s NED */ + float vy; /**< in m/s NED */ + float vz; /**< in m/s NED */ +}; /**< Velocity setpoint in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_velocity_setpoint); + +#endif -- cgit v1.2.3 From eb5af244b9281e8d654d5f87feedde3e652b5fc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 21:57:46 +0400 Subject: sdlog2: GVSP (global velocity setpoint) message added, cleanup --- src/modules/sdlog2/sdlog2.c | 70 ++++++++++++++++-------------------- src/modules/sdlog2/sdlog2_messages.h | 9 +++++ 2 files changed, 40 insertions(+), 39 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec1b7628d..6641d88f3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -94,7 +95,6 @@ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ - /*printf("skip\n");*/ \ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ @@ -102,9 +102,6 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; - -//#define SDLOG2_DEBUG - static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("sdlog2 already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (!thread_running) { - printf("\tsdlog2 is not started\n"); + warnx("not started"); } main_thread_should_exit = true; @@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[]) sdlog2_status(); } else { - printf("\tsdlog2 not started\n"); + warnx("not started\n"); } exit(0); @@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); -#ifdef SDLOG2_DEBUG - int rp = logbuf->read_ptr; - int wp = logbuf->write_ptr; -#endif - /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg) n = write(log_file, read_ptr, n); should_wait = (n == available) && !is_part; -#ifdef SDLOG2_DEBUG - printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); -#endif if (n < 0) { main_thread_should_exit = true; @@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg) } else { n = 0; -#ifdef SDLOG2_DEBUG - printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); -#endif /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { -#ifdef SDLOG2_DEBUG - printf("break logwriter thread\n"); -#endif break; } should_wait = true; @@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg) fsync(log_file); close(log_file); -#ifdef SDLOG2_DEBUG - printf("logwriter thread exit\n"); -#endif - return OK; } @@ -614,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 19; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); @@ -646,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; + struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; memset(&buf, 0, sizeof(buf)); @@ -669,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rc_sub; int airspeed_sub; int esc_sub; + int global_vel_sp_sub; } subs; /* log message buffer: header + body */ @@ -694,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; + struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -701,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 20; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; @@ -815,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL VELOCITY SETPOINT --- */ + subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + fds[fdsc_count].fd = subs.global_vel_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1166,14 +1154,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } } -#ifdef SDLOG2_DEBUG - printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { -#ifdef SDLOG2_DEBUG - printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e479b524..06e640b5b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -244,6 +244,14 @@ struct log_ESC_s { uint16_t esc_setpoint_raw; }; +/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ +#define LOG_GVSP_MSG 19 +struct log_GVSP_s { + float vx; + float vy; + float vz; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -267,6 +275,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); -- cgit v1.2.3 From 87782300509572ea593f309cbbf0a1ca64a53775 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 21:59:49 +0400 Subject: multirotor_pos_control fixes, systemlib/pid now accepts limit = 0.0, means no limit --- .../multirotor_pos_control.c | 147 +++++++++++++-------- src/modules/systemlib/pid/pid.c | 14 +- 2 files changed, 103 insertions(+), 58 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3e5857c26..1d2dd384a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -89,9 +90,11 @@ static float scale_control(float ctl, float end, float dz); static float norm(float x, float y); -static void usage(const char *reason) { +static void usage(const char *reason) +{ if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -100,11 +103,12 @@ static void usage(const char *reason) { * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) { +int multirotor_pos_control_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -119,11 +123,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) { warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -136,9 +140,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("app is running"); + } else { warnx("app not started"); } + exit(0); } @@ -146,26 +152,31 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } -static float scale_control(float ctl, float end, float dz) { +static float scale_control(float ctl, float end, float dz) +{ if (ctl > dz) { return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { return (ctl + dz) / (end - dz); + } else { return 0.0f; } } -static float norm(float x, float y) { +static float norm(float x, float y) +{ return sqrtf(x * x + y * y); } -static int multirotor_pos_control_thread_main(int argc, char *argv[]) { +static int multirotor_pos_control_thread_main(int argc, char *argv[]) +{ /* welcome user */ warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); + mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ struct vehicle_status_s status; @@ -181,7 +192,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + memset(&global_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -195,6 +208,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); + orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); bool reset_sp_alt = true; @@ -220,10 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -234,18 +249,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* check parameters at 1 Hz*/ paramcheck_counter++; + if (paramcheck_counter == 50) { bool param_updated; orb_check(param_sub, ¶m_updated); + if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); } + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } + paramcheck_counter = 0; } @@ -256,31 +276,36 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* Check if controller should act */ bool act = status.flag_system_armed && ( - /* SAS modes */ - ( - status.flag_control_manual_enabled && - status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( - status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || - status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE - ) - ) || - /* AUTO mode */ - status.state_machine == SYSTEM_STATE_AUTO - ); + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); hrt_abstime t = hrt_absolute_time(); float dt; + if (t_prev != 0) { dt = (t - t_prev) * 0.000001f; + } else { dt = 0.0f; } + t_prev = t; + if (act) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); if (local_pos.home_timestamp != local_home_timestamp) { @@ -292,6 +317,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); } + if (global_pos_sp_updated) { global_pos_sp_updated = false; orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); @@ -299,11 +325,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { double sp_lon = global_pos_sp.lon * 1e-7; /* project global setpoint to local setpoint */ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp.altitude_is_relative) { local_pos_sp.z = -global_pos_sp.altitude; + } else { local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; } + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); /* publish local position setpoint as projection of global position setpoint */ @@ -339,16 +368,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* home alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.home_alt - home_alt; } + home_alt_t = local_pos.home_timestamp; home_alt = local_pos.home_alt; } + /* move altitude setpoint with manual controls */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { local_pos_sp.z = local_pos.z - z_sp_offs_max; } @@ -358,6 +392,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* move position setpoint with manual controls */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); @@ -370,6 +405,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; + if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; @@ -379,72 +415,79 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } /* run position & altitude controllers, calculate velocity setpoint */ - float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; - vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); - vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + + /* limit horizontal speed */ + float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { + global_vel_sp.vx /= xy_vel_sp_norm; + global_vel_sp.vy /= xy_vel_sp_norm; + } + } else { reset_sp_pos = true; + global_vel_sp.vx = 0.0f; + global_vel_sp.vy = 0.0f; } - /* calculate direction and norm of thrust in NED frame - * limit 3D speed by ellipsoid: - * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ - float v; - float vel_sp_norm = 0.0f; - v = vel_sp[0] / params.xy_vel_max; - vel_sp_norm += v * v; - v = vel_sp[1] / params.xy_vel_max; - vel_sp_norm += v * v; - v = vel_sp[2] / params.z_vel_max; - vel_sp_norm += v * v; - vel_sp_norm = sqrtf(vel_sp_norm); - if (vel_sp_norm > 1.0f) { - vel_sp[0] /= vel_sp_norm; - vel_sp[1] /= vel_sp_norm; - vel_sp[2] /= vel_sp_norm; - } + + /* publish new velocity setpoint */ + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); /* run velocity controllers, calculate thrust vector */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); } + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { thrust_xy_norm = params.slope_max; } + /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch } + /* attitude-thrust compensation */ float att_comp; + if (att.R[2][2] > 0.8f) att_comp = 1.0f / att.R[2][2]; else if (att.R[2][2] > 0.0f) att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; else att_comp = 1.0f; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else { reset_sp_alt = true; reset_sp_pos = true; @@ -456,7 +499,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f66..562f3ac04 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -168,8 +168,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,11 +186,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit != 0.0f) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; -- cgit v1.2.3 From e67f6a51a320a9b8bf0a27c4ffb55a9485805680 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 20:13:53 -0700 Subject: Make px4io driver filenames less ambiguous. --- src/drivers/px4io/interface_i2c.cpp | 187 ---------- src/drivers/px4io/interface_serial.cpp | 655 --------------------------------- src/drivers/px4io/module.mk | 6 +- src/drivers/px4io/px4io_i2c.cpp | 187 ++++++++++ src/drivers/px4io/px4io_serial.cpp | 655 +++++++++++++++++++++++++++++++++ src/drivers/px4io/px4io_uploader.cpp | 606 ++++++++++++++++++++++++++++++ src/drivers/px4io/uploader.cpp | 606 ------------------------------ 7 files changed, 1451 insertions(+), 1451 deletions(-) delete mode 100644 src/drivers/px4io/interface_i2c.cpp delete mode 100644 src/drivers/px4io/interface_serial.cpp create mode 100644 src/drivers/px4io/px4io_i2c.cpp create mode 100644 src/drivers/px4io/px4io_serial.cpp create mode 100644 src/drivers/px4io/px4io_uploader.cpp delete mode 100644 src/drivers/px4io/uploader.cpp diff --git a/src/drivers/px4io/interface_i2c.cpp b/src/drivers/px4io/interface_i2c.cpp deleted file mode 100644 index 45a707883..000000000 --- a/src/drivers/px4io/interface_i2c.cpp +++ /dev/null @@ -1,187 +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 interface_i2c.cpp - * - * I2C interface for PX4IO - */ - -/* XXX trim includes */ -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include "uploader.h" -#include - -#include "interface.h" - -class PX4IO_I2C : public PX4IO_interface -{ -public: - PX4IO_I2C(int bus, uint8_t address); - virtual ~PX4IO_I2C(); - - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); - - virtual bool ok(); - - virtual int test(unsigned mode); - -private: - static const unsigned _retries = 2; - - struct i2c_dev_s *_dev; - uint8_t _address; -}; - -PX4IO_interface *io_i2c_interface(int bus, uint8_t address) -{ - return new PX4IO_I2C(bus, address); -} - -PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - _dev(nullptr), - _address(address) -{ - _dev = up_i2cinitialize(bus); - if (_dev) - I2C_SETFREQUENCY(_dev, 400000); -} - -PX4IO_I2C::~PX4IO_I2C() -{ - if (_dev) - up_i2cuninitialize(_dev); -} - -bool -PX4IO_I2C::ok() -{ - if (!_dev) - return false; - - /* check any other status here */ - - return true; -} - -int -PX4IO_I2C::test(unsigned mode) -{ - return 0; -} - -int -PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) -{ - int ret; - - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].addr = _address; - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - - msgv[1].addr = _address; - msgv[1].flags = I2C_M_NORESTART; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - unsigned tries = 0; - do { - - /* perform the transfer */ - ret = I2C_TRANSFER(_dev, msgv, 2); - - if (ret == OK) - break; - - } while (tries++ < _retries); - - return ret; -} - -int -PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) -{ - int ret; - - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; - - msgv[0].addr = _address; - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - - msgv[1].addr = _address; - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - unsigned tries = 0; - do { - /* perform the transfer */ - ret = I2C_TRANSFER(_dev, msgv, 2); - - if (ret == OK) - break; - - } while (tries++ < _retries); - - return ret; -} diff --git a/src/drivers/px4io/interface_serial.cpp b/src/drivers/px4io/interface_serial.cpp deleted file mode 100644 index 06b955971..000000000 --- a/src/drivers/px4io/interface_serial.cpp +++ /dev/null @@ -1,655 +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 interface_serial.cpp - * - * Serial interface for PX4IO - */ - -/* XXX trim includes */ -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -/* XXX might be able to prune these */ -#include -#include -#include -#include - -#include - -#include -#include /* XXX should really not be hardcoding v2 here */ - -#include - -#include - -#include "interface.h" - -/* serial register accessors */ -#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) -#define rSR REG(STM32_USART_SR_OFFSET) -#define rDR REG(STM32_USART_DR_OFFSET) -#define rBRR REG(STM32_USART_BRR_OFFSET) -#define rCR1 REG(STM32_USART_CR1_OFFSET) -#define rCR2 REG(STM32_USART_CR2_OFFSET) -#define rCR3 REG(STM32_USART_CR3_OFFSET) -#define rGTPR REG(STM32_USART_GTPR_OFFSET) - -class PX4IO_serial : public PX4IO_interface -{ -public: - PX4IO_serial(); - virtual ~PX4IO_serial(); - - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); - - virtual bool ok(); - virtual int test(unsigned mode); - -private: - /* - * XXX tune this value - * - * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. - * Packet overhead is 26µs for the four-byte header. - * - * 32 registers = 451µs - * - * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) - * transfers? Could cause issues with any regs expecting to be written atomically... - */ - static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory - - DMA_HANDLE _tx_dma; - DMA_HANDLE _rx_dma; - - /** saved DMA status */ - static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values - static const unsigned _dma_status_waiting = 0x00000000; - volatile unsigned _rx_dma_status; - - /** bus-ownership lock */ - sem_t _bus_semaphore; - - /** client-waiting lock/signal */ - sem_t _completion_semaphore; - - /** - * Start the transaction with IO and wait for it to complete. - */ - int _wait_complete(); - - /** - * DMA completion handler. - */ - static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); - void _do_rx_dma_callback(unsigned status); - - /** - * Serial interrupt handler. - */ - static int _interrupt(int vector, void *context); - void _do_interrupt(); - - /** - * Cancel any DMA in progress with an error. - */ - void _abort_dma(); - - /** - * Performance counters. - */ - perf_counter_t _pc_txns; - perf_counter_t _pc_dmasetup; - perf_counter_t _pc_retries; - perf_counter_t _pc_timeouts; - perf_counter_t _pc_crcerrs; - perf_counter_t _pc_dmaerrs; - perf_counter_t _pc_protoerrs; - perf_counter_t _pc_uerrs; - perf_counter_t _pc_idle; - perf_counter_t _pc_badidle; - -}; - -IOPacket PX4IO_serial::_dma_buffer; -static PX4IO_serial *g_interface; - -PX4IO_interface *io_serial_interface() -{ - return new PX4IO_serial(); -} - -PX4IO_serial::PX4IO_serial() : - _tx_dma(nullptr), - _rx_dma(nullptr), - _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "txns ")), - _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), - _pc_retries(perf_alloc(PC_COUNT, "retries ")), - _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), - _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), - _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), - _pc_uerrs(perf_alloc(PC_COUNT, "uarterrs ")), - _pc_idle(perf_alloc(PC_COUNT, "idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "badidle ")) -{ - /* allocate DMA */ - _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); - _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); - if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) - return; - - /* configure pins for serial use */ - stm32_configgpio(PX4IO_SERIAL_TX_GPIO); - stm32_configgpio(PX4IO_SERIAL_RX_GPIO); - - /* reset & configure the UART */ - rCR1 = 0; - rCR2 = 0; - rCR3 = 0; - - /* eat any existing interrupt status */ - (void)rSR; - (void)rDR; - - /* configure line speed */ - uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); - uint32_t mantissa = usartdiv32 >> 5; - uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; - rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); - - /* attach serial interrupt handler */ - irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); - up_enable_irq(PX4IO_SERIAL_VECTOR); - - /* enable UART in DMA mode, enable error and line idle interrupts */ - /* rCR3 = USART_CR3_EIE;*/ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; - - /* create semaphores */ - sem_init(&_completion_semaphore, 0, 0); - sem_init(&_bus_semaphore, 0, 1); - - g_interface = this; -} - -PX4IO_serial::~PX4IO_serial() -{ - if (_tx_dma != nullptr) { - stm32_dmastop(_tx_dma); - stm32_dmafree(_tx_dma); - } - if (_rx_dma != nullptr) { - stm32_dmastop(_rx_dma); - stm32_dmafree(_rx_dma); - } - - /* reset the UART */ - rCR1 = 0; - rCR2 = 0; - rCR3 = 0; - - /* detach our interrupt handler */ - up_disable_irq(PX4IO_SERIAL_VECTOR); - irq_detach(PX4IO_SERIAL_VECTOR); - - /* restore the GPIOs */ - stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); - stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); - - /* and kill our semaphores */ - sem_destroy(&_completion_semaphore); - sem_destroy(&_bus_semaphore); - - perf_free(_pc_txns); - perf_free(_pc_dmasetup); - perf_free(_pc_retries); - perf_free(_pc_timeouts); - perf_free(_pc_crcerrs); - perf_free(_pc_dmaerrs); - perf_free(_pc_protoerrs); - perf_free(_pc_uerrs); - perf_free(_pc_idle); - perf_free(_pc_badidle); - - if (g_interface == this) - g_interface = nullptr; -} - -bool -PX4IO_serial::ok() -{ - if (_tx_dma == nullptr) - return false; - if (_rx_dma == nullptr) - return false; - - return true; -} - -int -PX4IO_serial::test(unsigned mode) -{ - - switch (mode) { - case 0: - lowsyslog("test 0\n"); - - /* kill DMA, this is a PIO test */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); - rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); - - for (;;) { - while (!(rSR & USART_SR_TXE)) - ; - rDR = 0x55; - } - return 0; - - case 1: - { - unsigned fails = 0; - for (unsigned count = 0;; count++) { - uint16_t value = count & 0xffff; - - if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) - fails++; - - if (count >= 5000) { - lowsyslog("==== test 1 : %u failures ====\n", fails); - perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); - perf_print_counter(_pc_retries); - perf_print_counter(_pc_timeouts); - perf_print_counter(_pc_crcerrs); - perf_print_counter(_pc_dmaerrs); - perf_print_counter(_pc_protoerrs); - perf_print_counter(_pc_uerrs); - perf_print_counter(_pc_idle); - perf_print_counter(_pc_badidle); - count = 0; - } - } - return 0; - } - case 2: - lowsyslog("test 2\n"); - return 0; - } - return -1; -} - -int -PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) -{ - if (num_values > PKT_MAX_REGS) { - errno = EINVAL; - return -1; - } - - sem_wait(&_bus_semaphore); - - int result; - for (unsigned retries = 0; retries < 3; retries++) { - - _dma_buffer.count_code = num_values | PKT_CODE_WRITE; - _dma_buffer.page = page; - _dma_buffer.offset = offset; - memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < PKT_MAX_REGS; i++) - _dma_buffer.regs[i] = 0x55aa; - - /* XXX implement check byte */ - - /* start the transaction and wait for it to complete */ - result = _wait_complete(); - - /* successful transaction? */ - if (result == OK) { - - /* check result in packet */ - if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { - - /* IO didn't like it - no point retrying */ - errno = EINVAL; - result = -1; - perf_count(_pc_protoerrs); - } - - break; - } - perf_count(_pc_retries); - } - - sem_post(&_bus_semaphore); - return result; -} - -int -PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) -{ - if (num_values > PKT_MAX_REGS) - return -EINVAL; - - sem_wait(&_bus_semaphore); - - int result; - for (unsigned retries = 0; retries < 3; retries++) { - - _dma_buffer.count_code = num_values | PKT_CODE_READ; - _dma_buffer.page = page; - _dma_buffer.offset = offset; - - /* start the transaction and wait for it to complete */ - result = _wait_complete(); - - /* successful transaction? */ - if (result == OK) { - - /* check result in packet */ - if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { - - /* IO didn't like it - no point retrying */ - errno = EINVAL; - result = -1; - perf_count(_pc_protoerrs); - - /* compare the received count with the expected count */ - } else if (PKT_COUNT(_dma_buffer) != num_values) { - - /* IO returned the wrong number of registers - no point retrying */ - errno = EIO; - result = -1; - perf_count(_pc_protoerrs); - - /* successful read */ - } else { - - /* copy back the result */ - memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); - } - - break; - } - perf_count(_pc_retries); - } -out: - sem_post(&_bus_semaphore); - return result; -} - -int -PX4IO_serial::_wait_complete() -{ - /* clear any lingering error status */ - (void)rSR; - (void)rDR; - - /* start RX DMA */ - perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); - - /* DMA setup time ~3µs */ - _rx_dma_status = _dma_status_waiting; - - /* - * Note that we enable circular buffer mode as a workaround for - * there being no API to disable the DMA FIFO. We need direct mode - * because otherwise when the line idle interrupt fires there - * will be packet bytes still in the DMA FIFO, and we will assume - * that the idle was spurious. - * - * XXX this should be fixed with a NuttX change. - */ - stm32_dmasetup( - _rx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - sizeof(_dma_buffer), - DMA_SCR_CIRC | /* XXX see note above */ - DMA_SCR_DIR_P2M | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); - stm32_dmastart(_rx_dma, _dma_callback, this, false); - rCR3 |= USART_CR3_DMAR; - - /* start TX DMA - no callback if we also expect a reply */ - /* DMA setup time ~3µs */ - _dma_buffer.crc = 0; - _dma_buffer.crc = crc_packet(&_dma_buffer); - stm32_dmasetup( - _tx_dma, - PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, - reinterpret_cast(&_dma_buffer), - PKT_SIZE(_dma_buffer), - DMA_SCR_DIR_M2P | - DMA_SCR_MINC | - DMA_SCR_PSIZE_8BITS | - DMA_SCR_MSIZE_8BITS | - DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); - stm32_dmastart(_tx_dma, nullptr, nullptr, false); - rCR3 |= USART_CR3_DMAT; - - perf_end(_pc_dmasetup); - - /* compute the deadline for a 5ms timeout */ - struct timespec abstime; - clock_gettime(CLOCK_REALTIME, &abstime); -#if 0 - abstime.tv_sec++; /* long timeout for testing */ -#else - abstime.tv_nsec += 10000000; /* 0ms timeout */ - if (abstime.tv_nsec > 1000000000) { - abstime.tv_sec++; - abstime.tv_nsec -= 1000000000; - } -#endif - - /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ - int ret; - for (;;) { - ret = sem_timedwait(&_completion_semaphore, &abstime); - - if (ret == OK) { - /* check for DMA errors */ - if (_rx_dma_status & DMA_STATUS_TEIF) { - perf_count(_pc_dmaerrs); - ret = -1; - errno = EIO; - break; - } - - /* check packet CRC - corrupt packet errors mean IO receive CRC error */ - uint8_t crc = _dma_buffer.crc; - _dma_buffer.crc = 0; - if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { - perf_count(_pc_crcerrs); - ret = -1; - errno = EIO; - break; - } - - /* successful txn (may still be reporting an error) */ - break; - } - - if (errno == ETIMEDOUT) { - /* something has broken - clear out any partial DMA state and reconfigure */ - _abort_dma(); - perf_count(_pc_timeouts); - perf_cancel(_pc_txns); /* don't count this as a transaction */ - break; - } - - /* we might? see this for EINTR */ - lowsyslog("unexpected ret %d/%d\n", ret, errno); - } - - /* reset DMA status */ - _rx_dma_status = _dma_status_inactive; - - /* update counters */ - perf_end(_pc_txns); - - return ret; -} - -void -PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) -{ - if (arg != nullptr) { - PX4IO_serial *ps = reinterpret_cast(arg); - - ps->_do_rx_dma_callback(status); - } -} - -void -PX4IO_serial::_do_rx_dma_callback(unsigned status) -{ - /* on completion of a reply, wake the waiter */ - if (_rx_dma_status == _dma_status_waiting) { - - /* check for packet overrun - this will occur after DMA completes */ - uint32_t sr = rSR; - if (sr & (USART_SR_ORE | USART_SR_RXNE)) { - (void)rDR; - status = DMA_STATUS_TEIF; - } - - /* save RX status */ - _rx_dma_status = status; - - /* disable UART DMA */ - rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - - /* complete now */ - sem_post(&_completion_semaphore); - } -} - -int -PX4IO_serial::_interrupt(int irq, void *context) -{ - if (g_interface != nullptr) - g_interface->_do_interrupt(); - return 0; -} - -void -PX4IO_serial::_do_interrupt() -{ - uint32_t sr = rSR; /* get UART status register */ - (void)rDR; /* read DR to clear status */ - - if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ - - /* - * If we are in the process of listening for something, these are all fatal; - * abort the DMA with an error. - */ - if (_rx_dma_status == _dma_status_waiting) { - _abort_dma(); - perf_count(_pc_uerrs); - - /* complete DMA as though in error */ - _do_rx_dma_callback(DMA_STATUS_TEIF); - - return; - } - - /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ - - /* don't attempt to handle IDLE if it's set - things went bad */ - return; - } - - if (sr & USART_SR_IDLE) { - - /* if there is DMA reception going on, this is a short packet */ - if (_rx_dma_status == _dma_status_waiting) { - - /* verify that the received packet is complete */ - unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); - if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { - perf_count(_pc_badidle); - return; - } - - perf_count(_pc_idle); - - /* stop the receive DMA */ - stm32_dmastop(_rx_dma); - - /* complete the short reception */ - _do_rx_dma_callback(DMA_STATUS_TCIF); - } - } -} - -void -PX4IO_serial::_abort_dma() -{ - /* disable UART DMA */ - rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - (void)rSR; - (void)rDR; - (void)rDR; - - /* stop DMA */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); -} diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index d5bab6599..2054faa12 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -38,9 +38,9 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ - uploader.cpp \ - interface_serial.cpp \ - interface_i2c.cpp + px4io_uploader.cpp \ + px4io_serial.cpp \ + px4io_i2c.cpp # XXX prune to just get UART registers INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp new file mode 100644 index 000000000..45a707883 --- /dev/null +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * 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 interface_i2c.cpp + * + * I2C interface for PX4IO + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include "uploader.h" +#include + +#include "interface.h" + +class PX4IO_I2C : public PX4IO_interface +{ +public: + PX4IO_I2C(int bus, uint8_t address); + virtual ~PX4IO_I2C(); + + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + virtual bool ok(); + + virtual int test(unsigned mode); + +private: + static const unsigned _retries = 2; + + struct i2c_dev_s *_dev; + uint8_t _address; +}; + +PX4IO_interface *io_i2c_interface(int bus, uint8_t address) +{ + return new PX4IO_I2C(bus, address); +} + +PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : + _dev(nullptr), + _address(address) +{ + _dev = up_i2cinitialize(bus); + if (_dev) + I2C_SETFREQUENCY(_dev, 400000); +} + +PX4IO_I2C::~PX4IO_I2C() +{ + if (_dev) + up_i2cuninitialize(_dev); +} + +bool +PX4IO_I2C::ok() +{ + if (!_dev) + return false; + + /* check any other status here */ + + return true; +} + +int +PX4IO_I2C::test(unsigned mode) +{ + return 0; +} + +int +PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + int ret; + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].addr = _address; + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].addr = _address; + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + unsigned tries = 0; + do { + + /* perform the transfer */ + ret = I2C_TRANSFER(_dev, msgv, 2); + + if (ret == OK) + break; + + } while (tries++ < _retries); + + return ret; +} + +int +PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + int ret; + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].addr = _address; + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].addr = _address; + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + unsigned tries = 0; + do { + /* perform the transfer */ + ret = I2C_TRANSFER(_dev, msgv, 2); + + if (ret == OK) + break; + + } while (tries++ < _retries); + + return ret; +} diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp new file mode 100644 index 000000000..06b955971 --- /dev/null +++ b/src/drivers/px4io/px4io_serial.cpp @@ -0,0 +1,655 @@ +/**************************************************************************** + * + * 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 interface_serial.cpp + * + * Serial interface for PX4IO + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +/* XXX might be able to prune these */ +#include +#include +#include +#include + +#include + +#include +#include /* XXX should really not be hardcoding v2 here */ + +#include + +#include + +#include "interface.h" + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +class PX4IO_serial : public PX4IO_interface +{ +public: + PX4IO_serial(); + virtual ~PX4IO_serial(); + + virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + virtual bool ok(); + virtual int test(unsigned mode); + +private: + /* + * XXX tune this value + * + * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. + * Packet overhead is 26µs for the four-byte header. + * + * 32 registers = 451µs + * + * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) + * transfers? Could cause issues with any regs expecting to be written atomically... + */ + static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory + + DMA_HANDLE _tx_dma; + DMA_HANDLE _rx_dma; + + /** saved DMA status */ + static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values + static const unsigned _dma_status_waiting = 0x00000000; + volatile unsigned _rx_dma_status; + + /** bus-ownership lock */ + sem_t _bus_semaphore; + + /** client-waiting lock/signal */ + sem_t _completion_semaphore; + + /** + * Start the transaction with IO and wait for it to complete. + */ + int _wait_complete(); + + /** + * DMA completion handler. + */ + static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); + void _do_rx_dma_callback(unsigned status); + + /** + * Serial interrupt handler. + */ + static int _interrupt(int vector, void *context); + void _do_interrupt(); + + /** + * Cancel any DMA in progress with an error. + */ + void _abort_dma(); + + /** + * Performance counters. + */ + perf_counter_t _pc_txns; + perf_counter_t _pc_dmasetup; + perf_counter_t _pc_retries; + perf_counter_t _pc_timeouts; + perf_counter_t _pc_crcerrs; + perf_counter_t _pc_dmaerrs; + perf_counter_t _pc_protoerrs; + perf_counter_t _pc_uerrs; + perf_counter_t _pc_idle; + perf_counter_t _pc_badidle; + +}; + +IOPacket PX4IO_serial::_dma_buffer; +static PX4IO_serial *g_interface; + +PX4IO_interface *io_serial_interface() +{ + return new PX4IO_serial(); +} + +PX4IO_serial::PX4IO_serial() : + _tx_dma(nullptr), + _rx_dma(nullptr), + _rx_dma_status(_dma_status_inactive), + _pc_txns(perf_alloc(PC_ELAPSED, "txns ")), + _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "retries ")), + _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "uarterrs ")), + _pc_idle(perf_alloc(PC_COUNT, "idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "badidle ")) +{ + /* allocate DMA */ + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) + return; + + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); + + /* reset & configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* enable UART in DMA mode, enable error and line idle interrupts */ + /* rCR3 = USART_CR3_EIE;*/ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + + /* create semaphores */ + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); + + g_interface = this; +} + +PX4IO_serial::~PX4IO_serial() +{ + if (_tx_dma != nullptr) { + stm32_dmastop(_tx_dma); + stm32_dmafree(_tx_dma); + } + if (_rx_dma != nullptr) { + stm32_dmastop(_rx_dma); + stm32_dmafree(_rx_dma); + } + + /* reset the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + + /* restore the GPIOs */ + stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + + /* and kill our semaphores */ + sem_destroy(&_completion_semaphore); + sem_destroy(&_bus_semaphore); + + perf_free(_pc_txns); + perf_free(_pc_dmasetup); + perf_free(_pc_retries); + perf_free(_pc_timeouts); + perf_free(_pc_crcerrs); + perf_free(_pc_dmaerrs); + perf_free(_pc_protoerrs); + perf_free(_pc_uerrs); + perf_free(_pc_idle); + perf_free(_pc_badidle); + + if (g_interface == this) + g_interface = nullptr; +} + +bool +PX4IO_serial::ok() +{ + if (_tx_dma == nullptr) + return false; + if (_rx_dma == nullptr) + return false; + + return true; +} + +int +PX4IO_serial::test(unsigned mode) +{ + + switch (mode) { + case 0: + lowsyslog("test 0\n"); + + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); + + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0x55; + } + return 0; + + case 1: + { + unsigned fails = 0; + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; + + if (count >= 5000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + count = 0; + } + } + return 0; + } + case 2: + lowsyslog("test 2\n"); + return 0; + } + return -1; +} + +int +PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + if (num_values > PKT_MAX_REGS) { + errno = EINVAL; + return -1; + } + + sem_wait(&_bus_semaphore); + + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = num_values | PKT_CODE_WRITE; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); + for (unsigned i = num_values; i < PKT_MAX_REGS; i++) + _dma_buffer.regs[i] = 0x55aa; + + /* XXX implement check byte */ + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + errno = EINVAL; + result = -1; + perf_count(_pc_protoerrs); + } + + break; + } + perf_count(_pc_retries); + } + + sem_post(&_bus_semaphore); + return result; +} + +int +PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + if (num_values > PKT_MAX_REGS) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = num_values | PKT_CODE_READ; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + errno = EINVAL; + result = -1; + perf_count(_pc_protoerrs); + + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != num_values) { + + /* IO returned the wrong number of registers - no point retrying */ + errno = EIO; + result = -1; + perf_count(_pc_protoerrs); + + /* successful read */ + } else { + + /* copy back the result */ + memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + } + + break; + } + perf_count(_pc_retries); + } +out: + sem_post(&_bus_semaphore); + return result; +} + +int +PX4IO_serial::_wait_complete() +{ + /* clear any lingering error status */ + (void)rSR; + (void)rDR; + + /* start RX DMA */ + perf_begin(_pc_txns); + perf_begin(_pc_dmasetup); + + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + + /* + * Note that we enable circular buffer mode as a workaround for + * there being no API to disable the DMA FIFO. We need direct mode + * because otherwise when the line idle interrupt fires there + * will be packet bytes still in the DMA FIFO, and we will assume + * that the idle was spurious. + * + * XXX this should be fixed with a NuttX change. + */ + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_CIRC | /* XXX see note above */ + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; + + /* start TX DMA - no callback if we also expect a reply */ + /* DMA setup time ~3µs */ + _dma_buffer.crc = 0; + _dma_buffer.crc = crc_packet(&_dma_buffer); + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast(&_dma_buffer), + PKT_SIZE(_dma_buffer), + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_tx_dma, nullptr, nullptr, false); + rCR3 |= USART_CR3_DMAT; + + perf_end(_pc_dmasetup); + + /* compute the deadline for a 5ms timeout */ + struct timespec abstime; + clock_gettime(CLOCK_REALTIME, &abstime); +#if 0 + abstime.tv_sec++; /* long timeout for testing */ +#else + abstime.tv_nsec += 10000000; /* 0ms timeout */ + if (abstime.tv_nsec > 1000000000) { + abstime.tv_sec++; + abstime.tv_nsec -= 1000000000; + } +#endif + + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); + + if (ret == OK) { + /* check for DMA errors */ + if (_rx_dma_status & DMA_STATUS_TEIF) { + perf_count(_pc_dmaerrs); + ret = -1; + errno = EIO; + break; + } + + /* check packet CRC - corrupt packet errors mean IO receive CRC error */ + uint8_t crc = _dma_buffer.crc; + _dma_buffer.crc = 0; + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { + perf_count(_pc_crcerrs); + ret = -1; + errno = EIO; + break; + } + + /* successful txn (may still be reporting an error) */ + break; + } + + if (errno == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _abort_dma(); + perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ + break; + } + + /* we might? see this for EINTR */ + lowsyslog("unexpected ret %d/%d\n", ret, errno); + } + + /* reset DMA status */ + _rx_dma_status = _dma_status_inactive; + + /* update counters */ + perf_end(_pc_txns); + + return ret; +} + +void +PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + if (arg != nullptr) { + PX4IO_serial *ps = reinterpret_cast(arg); + + ps->_do_rx_dma_callback(status); + } +} + +void +PX4IO_serial::_do_rx_dma_callback(unsigned status) +{ + /* on completion of a reply, wake the waiter */ + if (_rx_dma_status == _dma_status_waiting) { + + /* check for packet overrun - this will occur after DMA completes */ + uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { + (void)rDR; + status = DMA_STATUS_TEIF; + } + + /* save RX status */ + _rx_dma_status = status; + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* complete now */ + sem_post(&_completion_semaphore); + } +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + if (g_interface != nullptr) + g_interface->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* read DR to clear status */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* + * If we are in the process of listening for something, these are all fatal; + * abort the DMA with an error. + */ + if (_rx_dma_status == _dma_status_waiting) { + _abort_dma(); + perf_count(_pc_uerrs); + + /* complete DMA as though in error */ + _do_rx_dma_callback(DMA_STATUS_TEIF); + + return; + } + + /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ + + /* don't attempt to handle IDLE if it's set - things went bad */ + return; + } + + if (sr & USART_SR_IDLE) { + + /* if there is DMA reception going on, this is a short packet */ + if (_rx_dma_status == _dma_status_waiting) { + + /* verify that the received packet is complete */ + unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { + perf_count(_pc_badidle); + return; + } + + perf_count(_pc_idle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TCIF); + } + } +} + +void +PX4IO_serial::_abort_dma() +{ + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* stop DMA */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); +} diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp new file mode 100644 index 000000000..ec22a5e17 --- /dev/null +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -0,0 +1,606 @@ +/**************************************************************************** + * + * 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 uploader.cpp + * Firmware uploader for PX4IO + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "uploader.h" + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#include +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#include +#endif + +// define for comms logging +//#define UDEBUG + +PX4IO_Uploader::PX4IO_Uploader() : + _io_fd(-1), + _fw_fd(-1) +{ +} + +PX4IO_Uploader::~PX4IO_Uploader() +{ +} + +int +PX4IO_Uploader::upload(const char *filenames[]) +{ + int ret; + const char *filename = NULL; + size_t fw_size; + +#ifndef PX4IO_SERIAL_DEVICE +#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload +#endif + + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); + + if (_io_fd < 0) { + log("could not open interface"); + return -errno; + } + + /* adjust line speed to match bootloader */ + struct termios t; + tcgetattr(_io_fd, &t); + cfsetspeed(&t, 115200); + tcsetattr(_io_fd, TCSANOW, &t); + + /* look for the bootloader */ + ret = sync(); + + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + close(_io_fd); + _io_fd = -1; + return -EIO; + } + + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + + log("using firmware from %s", filenames[i]); + filename = filenames[i]; + break; + } + + if (filename == NULL) { + log("no firmware found"); + close(_io_fd); + _io_fd = -1; + return -ENOENT; + } + + struct stat st; + if (stat(filename, &st) != 0) { + log("Failed to stat %s - %d\n", filename, (int)errno); + close(_io_fd); + _io_fd = -1; + return -errno; + } + fw_size = st.st_size; + + if (_fw_fd == -1) { + close(_io_fd); + _io_fd = -1; + return -ENOENT; + } + + /* do the usual program thing - allow for failure */ + for (unsigned retries = 0; retries < 1; retries++) { + if (retries > 0) { + log("retrying update..."); + ret = sync(); + + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + close(_io_fd); + _io_fd = -1; + return -EIO; + } + } + + ret = get_info(INFO_BL_REV, bl_rev); + + if (ret == OK) { + if (bl_rev <= BL_REV) { + log("found bootloader revision: %d", bl_rev); + } else { + log("found unsupported bootloader revision %d, exiting", bl_rev); + close(_io_fd); + _io_fd = -1; + return OK; + } + } + + ret = erase(); + + if (ret != OK) { + log("erase failed"); + continue; + } + + ret = program(fw_size); + + if (ret != OK) { + log("program failed"); + continue; + } + + if (bl_rev <= 2) + ret = verify_rev2(fw_size); + else if(bl_rev == 3) { + ret = verify_rev3(fw_size); + } + + if (ret != OK) { + log("verify failed"); + continue; + } + + ret = reboot(); + + if (ret != OK) { + log("reboot failed"); + return ret; + } + + log("update complete"); + + ret = OK; + break; + } + + close(_fw_fd); + close(_io_fd); + _io_fd = -1; + return ret; +} + +int +PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +{ + struct pollfd fds[1]; + + fds[0].fd = _io_fd; + fds[0].events = POLLIN; + + /* wait 100 ms for a character */ + int ret = ::poll(&fds[0], 1, timeout); + + if (ret < 1) { +#ifdef UDEBUG + log("poll timeout %d", ret); +#endif + return -ETIMEDOUT; + } + + read(_io_fd, &c, 1); +#ifdef UDEBUG + log("recv 0x%02x", c); +#endif + return OK; +} + +int +PX4IO_Uploader::recv(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = recv(*p++, 5000); + + if (ret != OK) + return ret; + } + + return OK; +} + +void +PX4IO_Uploader::drain() +{ + uint8_t c; + int ret; + + do { + ret = recv(c, 1000); + +#ifdef UDEBUG + if (ret == OK) { + log("discard 0x%02x", c); + } +#endif + } while (ret == OK); +} + +int +PX4IO_Uploader::send(uint8_t c) +{ +#ifdef UDEBUG + log("send 0x%02x", c); +#endif + if (write(_io_fd, &c, 1) != 1) + return -errno; + + return OK; +} + +int +PX4IO_Uploader::send(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = send(*p++); + + if (ret != OK) + return ret; + } + + return OK; +} + +int +PX4IO_Uploader::get_sync(unsigned timeout) +{ + uint8_t c[2]; + int ret; + + ret = recv(c[0], timeout); + + if (ret != OK) + return ret; + + ret = recv(c[1], timeout); + + if (ret != OK) + return ret; + + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { + log("bad sync 0x%02x,0x%02x", c[0], c[1]); + return -EIO; + } + + return OK; +} + +int +PX4IO_Uploader::sync() +{ + drain(); + + /* complete any pending program operation */ + for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) + send(0); + + send(PROTO_GET_SYNC); + send(PROTO_EOC); + return get_sync(); +} + +int +PX4IO_Uploader::get_info(int param, uint32_t &val) +{ + int ret; + + send(PROTO_GET_DEVICE); + send(param); + send(PROTO_EOC); + + ret = recv((uint8_t *)&val, sizeof(val)); + + if (ret != OK) + return ret; + + return get_sync(); +} + +int +PX4IO_Uploader::erase() +{ + log("erase..."); + send(PROTO_CHIP_ERASE); + send(PROTO_EOC); + return get_sync(10000); /* allow 10s timeout */ +} + + +static int read_with_retry(int fd, void *buf, size_t n) +{ + int ret; + uint8_t retries = 0; + do { + ret = read(fd, buf, n); + } while (ret == -1 && retries++ < 100); + if (retries != 0) { + printf("read of %u bytes needed %u retries\n", + (unsigned)n, + (unsigned)retries); + } + return ret; +} + +int +PX4IO_Uploader::program(size_t fw_size) +{ + uint8_t file_buf[PROG_MULTI_MAX]; + ssize_t count; + int ret; + size_t sent = 0; + + log("programming %u bytes...", (unsigned)fw_size); + + ret = lseek(_fw_fd, 0, SEEK_SET); + + while (sent < fw_size) { + /* get more bytes to program */ + size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + } + + if (count == 0) + return OK; + + sent += count; + + if (count < 0) + return -errno; + + ASSERT((count % 4) == 0); + + send(PROTO_PROG_MULTI); + send(count); + send(&file_buf[0], count); + send(PROTO_EOC); + + ret = get_sync(1000); + + if (ret != OK) + return ret; + } + return OK; +} + +int +PX4IO_Uploader::verify_rev2(size_t fw_size) +{ + uint8_t file_buf[4]; + ssize_t count; + int ret; + size_t sent = 0; + + log("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + + if (ret != OK) + return ret; + + while (sent < fw_size) { + /* get more bytes to verify */ + size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + } + + if (count == 0) + break; + + sent += count; + + if (count < 0) + return -errno; + + ASSERT((count % 4) == 0); + + send(PROTO_READ_MULTI); + send(count); + send(PROTO_EOC); + + for (ssize_t i = 0; i < count; i++) { + uint8_t c; + + ret = recv(c, 5000); + + if (ret != OK) { + log("%d: got %d waiting for bytes", sent + i, ret); + return ret; + } + + if (c != file_buf[i]) { + log("%d: got 0x%02x expected 0x%02x", sent + i, c, file_buf[i]); + return -EINVAL; + } + } + + ret = get_sync(); + + if (ret != OK) { + log("timeout waiting for post-verify sync"); + return ret; + } + } + + return OK; +} + +int +PX4IO_Uploader::verify_rev3(size_t fw_size_local) +{ + int ret; + uint8_t file_buf[4]; + ssize_t count; + uint32_t sum = 0; + uint32_t bytes_read = 0; + uint32_t crc = 0; + uint32_t fw_size_remote; + uint8_t fill_blank = 0xff; + + log("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + ret = get_info(INFO_FLASH_SIZE, fw_size_remote); + send(PROTO_EOC); + + if (ret != OK) { + log("could not read firmware size"); + return ret; + } + + /* read through the firmware file again and calculate the checksum*/ + while (bytes_read < fw_size_local) { + size_t n = fw_size_local - bytes_read; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)bytes_read, + (int)count, + (int)errno); + } + + /* set the rest to ff */ + if (count == 0) { + break; + } + /* stop if the file cannot be read */ + if (count < 0) + return -errno; + + /* calculate crc32 sum */ + sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); + + bytes_read += count; + } + + /* fill the rest with 0xff */ + while (bytes_read < fw_size_remote) { + sum = crc32part(&fill_blank, sizeof(fill_blank), sum); + bytes_read += sizeof(fill_blank); + } + + /* request CRC from IO */ + send(PROTO_GET_CRC); + send(PROTO_EOC); + + ret = recv((uint8_t*)(&crc), sizeof(crc)); + + if (ret != OK) { + log("did not receive CRC checksum"); + return ret; + } + + /* compare the CRC sum from the IO with the one calculated */ + if (sum != crc) { + log("CRC wrong: received: %d, expected: %d", crc, sum); + return -EINVAL; + } + + return OK; +} + +int +PX4IO_Uploader::reboot() +{ + send(PROTO_REBOOT); + send(PROTO_EOC); + + return OK; +} + +void +PX4IO_Uploader::log(const char *fmt, ...) +{ + va_list ap; + + printf("[PX4IO] "); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp deleted file mode 100644 index ec22a5e17..000000000 --- a/src/drivers/px4io/uploader.cpp +++ /dev/null @@ -1,606 +0,0 @@ -/**************************************************************************** - * - * 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 uploader.cpp - * Firmware uploader for PX4IO - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "uploader.h" - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#include -#endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#include -#endif - -// define for comms logging -//#define UDEBUG - -PX4IO_Uploader::PX4IO_Uploader() : - _io_fd(-1), - _fw_fd(-1) -{ -} - -PX4IO_Uploader::~PX4IO_Uploader() -{ -} - -int -PX4IO_Uploader::upload(const char *filenames[]) -{ - int ret; - const char *filename = NULL; - size_t fw_size; - -#ifndef PX4IO_SERIAL_DEVICE -#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload -#endif - - _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); - - if (_io_fd < 0) { - log("could not open interface"); - return -errno; - } - - /* adjust line speed to match bootloader */ - struct termios t; - tcgetattr(_io_fd, &t); - cfsetspeed(&t, 115200); - tcsetattr(_io_fd, TCSANOW, &t); - - /* look for the bootloader */ - ret = sync(); - - if (ret != OK) { - /* this is immediately fatal */ - log("bootloader not responding"); - close(_io_fd); - _io_fd = -1; - return -EIO; - } - - for (unsigned i = 0; filenames[i] != nullptr; i++) { - _fw_fd = open(filenames[i], O_RDONLY); - - if (_fw_fd < 0) { - log("failed to open %s", filenames[i]); - continue; - } - - log("using firmware from %s", filenames[i]); - filename = filenames[i]; - break; - } - - if (filename == NULL) { - log("no firmware found"); - close(_io_fd); - _io_fd = -1; - return -ENOENT; - } - - struct stat st; - if (stat(filename, &st) != 0) { - log("Failed to stat %s - %d\n", filename, (int)errno); - close(_io_fd); - _io_fd = -1; - return -errno; - } - fw_size = st.st_size; - - if (_fw_fd == -1) { - close(_io_fd); - _io_fd = -1; - return -ENOENT; - } - - /* do the usual program thing - allow for failure */ - for (unsigned retries = 0; retries < 1; retries++) { - if (retries > 0) { - log("retrying update..."); - ret = sync(); - - if (ret != OK) { - /* this is immediately fatal */ - log("bootloader not responding"); - close(_io_fd); - _io_fd = -1; - return -EIO; - } - } - - ret = get_info(INFO_BL_REV, bl_rev); - - if (ret == OK) { - if (bl_rev <= BL_REV) { - log("found bootloader revision: %d", bl_rev); - } else { - log("found unsupported bootloader revision %d, exiting", bl_rev); - close(_io_fd); - _io_fd = -1; - return OK; - } - } - - ret = erase(); - - if (ret != OK) { - log("erase failed"); - continue; - } - - ret = program(fw_size); - - if (ret != OK) { - log("program failed"); - continue; - } - - if (bl_rev <= 2) - ret = verify_rev2(fw_size); - else if(bl_rev == 3) { - ret = verify_rev3(fw_size); - } - - if (ret != OK) { - log("verify failed"); - continue; - } - - ret = reboot(); - - if (ret != OK) { - log("reboot failed"); - return ret; - } - - log("update complete"); - - ret = OK; - break; - } - - close(_fw_fd); - close(_io_fd); - _io_fd = -1; - return ret; -} - -int -PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) -{ - struct pollfd fds[1]; - - fds[0].fd = _io_fd; - fds[0].events = POLLIN; - - /* wait 100 ms for a character */ - int ret = ::poll(&fds[0], 1, timeout); - - if (ret < 1) { -#ifdef UDEBUG - log("poll timeout %d", ret); -#endif - return -ETIMEDOUT; - } - - read(_io_fd, &c, 1); -#ifdef UDEBUG - log("recv 0x%02x", c); -#endif - return OK; -} - -int -PX4IO_Uploader::recv(uint8_t *p, unsigned count) -{ - while (count--) { - int ret = recv(*p++, 5000); - - if (ret != OK) - return ret; - } - - return OK; -} - -void -PX4IO_Uploader::drain() -{ - uint8_t c; - int ret; - - do { - ret = recv(c, 1000); - -#ifdef UDEBUG - if (ret == OK) { - log("discard 0x%02x", c); - } -#endif - } while (ret == OK); -} - -int -PX4IO_Uploader::send(uint8_t c) -{ -#ifdef UDEBUG - log("send 0x%02x", c); -#endif - if (write(_io_fd, &c, 1) != 1) - return -errno; - - return OK; -} - -int -PX4IO_Uploader::send(uint8_t *p, unsigned count) -{ - while (count--) { - int ret = send(*p++); - - if (ret != OK) - return ret; - } - - return OK; -} - -int -PX4IO_Uploader::get_sync(unsigned timeout) -{ - uint8_t c[2]; - int ret; - - ret = recv(c[0], timeout); - - if (ret != OK) - return ret; - - ret = recv(c[1], timeout); - - if (ret != OK) - return ret; - - if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { - log("bad sync 0x%02x,0x%02x", c[0], c[1]); - return -EIO; - } - - return OK; -} - -int -PX4IO_Uploader::sync() -{ - drain(); - - /* complete any pending program operation */ - for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) - send(0); - - send(PROTO_GET_SYNC); - send(PROTO_EOC); - return get_sync(); -} - -int -PX4IO_Uploader::get_info(int param, uint32_t &val) -{ - int ret; - - send(PROTO_GET_DEVICE); - send(param); - send(PROTO_EOC); - - ret = recv((uint8_t *)&val, sizeof(val)); - - if (ret != OK) - return ret; - - return get_sync(); -} - -int -PX4IO_Uploader::erase() -{ - log("erase..."); - send(PROTO_CHIP_ERASE); - send(PROTO_EOC); - return get_sync(10000); /* allow 10s timeout */ -} - - -static int read_with_retry(int fd, void *buf, size_t n) -{ - int ret; - uint8_t retries = 0; - do { - ret = read(fd, buf, n); - } while (ret == -1 && retries++ < 100); - if (retries != 0) { - printf("read of %u bytes needed %u retries\n", - (unsigned)n, - (unsigned)retries); - } - return ret; -} - -int -PX4IO_Uploader::program(size_t fw_size) -{ - uint8_t file_buf[PROG_MULTI_MAX]; - ssize_t count; - int ret; - size_t sent = 0; - - log("programming %u bytes...", (unsigned)fw_size); - - ret = lseek(_fw_fd, 0, SEEK_SET); - - while (sent < fw_size) { - /* get more bytes to program */ - size_t n = fw_size - sent; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); - } - count = read_with_retry(_fw_fd, file_buf, n); - - if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", - (unsigned)n, - (unsigned)sent, - (int)count, - (int)errno); - } - - if (count == 0) - return OK; - - sent += count; - - if (count < 0) - return -errno; - - ASSERT((count % 4) == 0); - - send(PROTO_PROG_MULTI); - send(count); - send(&file_buf[0], count); - send(PROTO_EOC); - - ret = get_sync(1000); - - if (ret != OK) - return ret; - } - return OK; -} - -int -PX4IO_Uploader::verify_rev2(size_t fw_size) -{ - uint8_t file_buf[4]; - ssize_t count; - int ret; - size_t sent = 0; - - log("verify..."); - lseek(_fw_fd, 0, SEEK_SET); - - send(PROTO_CHIP_VERIFY); - send(PROTO_EOC); - ret = get_sync(); - - if (ret != OK) - return ret; - - while (sent < fw_size) { - /* get more bytes to verify */ - size_t n = fw_size - sent; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); - } - count = read_with_retry(_fw_fd, file_buf, n); - - if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", - (unsigned)n, - (unsigned)sent, - (int)count, - (int)errno); - } - - if (count == 0) - break; - - sent += count; - - if (count < 0) - return -errno; - - ASSERT((count % 4) == 0); - - send(PROTO_READ_MULTI); - send(count); - send(PROTO_EOC); - - for (ssize_t i = 0; i < count; i++) { - uint8_t c; - - ret = recv(c, 5000); - - if (ret != OK) { - log("%d: got %d waiting for bytes", sent + i, ret); - return ret; - } - - if (c != file_buf[i]) { - log("%d: got 0x%02x expected 0x%02x", sent + i, c, file_buf[i]); - return -EINVAL; - } - } - - ret = get_sync(); - - if (ret != OK) { - log("timeout waiting for post-verify sync"); - return ret; - } - } - - return OK; -} - -int -PX4IO_Uploader::verify_rev3(size_t fw_size_local) -{ - int ret; - uint8_t file_buf[4]; - ssize_t count; - uint32_t sum = 0; - uint32_t bytes_read = 0; - uint32_t crc = 0; - uint32_t fw_size_remote; - uint8_t fill_blank = 0xff; - - log("verify..."); - lseek(_fw_fd, 0, SEEK_SET); - - ret = get_info(INFO_FLASH_SIZE, fw_size_remote); - send(PROTO_EOC); - - if (ret != OK) { - log("could not read firmware size"); - return ret; - } - - /* read through the firmware file again and calculate the checksum*/ - while (bytes_read < fw_size_local) { - size_t n = fw_size_local - bytes_read; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); - } - count = read_with_retry(_fw_fd, file_buf, n); - - if (count != (ssize_t)n) { - log("firmware read of %u bytes at %u failed -> %d errno %d", - (unsigned)n, - (unsigned)bytes_read, - (int)count, - (int)errno); - } - - /* set the rest to ff */ - if (count == 0) { - break; - } - /* stop if the file cannot be read */ - if (count < 0) - return -errno; - - /* calculate crc32 sum */ - sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); - - bytes_read += count; - } - - /* fill the rest with 0xff */ - while (bytes_read < fw_size_remote) { - sum = crc32part(&fill_blank, sizeof(fill_blank), sum); - bytes_read += sizeof(fill_blank); - } - - /* request CRC from IO */ - send(PROTO_GET_CRC); - send(PROTO_EOC); - - ret = recv((uint8_t*)(&crc), sizeof(crc)); - - if (ret != OK) { - log("did not receive CRC checksum"); - return ret; - } - - /* compare the CRC sum from the IO with the one calculated */ - if (sum != crc) { - log("CRC wrong: received: %d, expected: %d", crc, sum); - return -EINVAL; - } - - return OK; -} - -int -PX4IO_Uploader::reboot() -{ - send(PROTO_REBOOT); - send(PROTO_EOC); - - return OK; -} - -void -PX4IO_Uploader::log(const char *fmt, ...) -{ - va_list ap; - - printf("[PX4IO] "); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} -- cgit v1.2.3 From 26eb0b9d724654bd87edd9191e72a726f5ce240b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 21:00:03 -0700 Subject: Move the direct-access methods from CDev to Device… makes more sense that way. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/device/cdev.cpp | 21 ---------- src/drivers/device/device.cpp | 26 +++++++++++++ src/drivers/device/device.h | 90 +++++++++++++++++++++++++------------------ src/drivers/device/i2c.h | 12 +++--- 4 files changed, 83 insertions(+), 66 deletions(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index dcbac25e3..1972d2efc 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -395,25 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup) return cdev->poll(filp, fds, setup); } -int -CDev::read(unsigned offset, void *data, unsigned count) -{ - errno = ENODEV; - return -1; -} - -int -CDev::write(unsigned offset, void *data, unsigned count) -{ - errno = ENODEV; - return -1; -} - -int -CDev::ioctl(unsigned operation, unsigned &arg) -{ - errno = ENODEV; - return -1; -} - } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 04a5222c3..dd8074460 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -223,5 +223,31 @@ interrupt(int irq, void *context) return OK; } +int +Device::probe() +{ + return -1; +} + +int +Device::read(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +Device::write(unsigned offset, void *data, unsigned count) +{ + errno = ENODEV; + return -1; +} + +int +Device::ioctl(unsigned operation, unsigned &arg) +{ + errno = ENODEV; + return -1; +} } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 2cac86636..d69d1b2d6 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -68,11 +68,62 @@ namespace device __EXPORT class __EXPORT Device { public: + /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + /** * Interrupt handler. */ virtual void interrupt(void *ctx); /**< interrupt handler */ + /* + * Direct access methods. + */ + + /** + * Probe to test whether the device is present. + * + * @return Zero if present, < 0 (error) otherwise. + */ + virtual int probe(); + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int read(unsigned address, void *data, unsigned count = 1); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of registers to write, defaults to 1. + * @return count on success, < 0 on error. + */ + virtual int write(unsigned address, void *data, unsigned count = 1); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform + * @param arg An argument to the operation. + * @return < 0 on error + */ + virtual int ioctl(unsigned operation, unsigned &arg); + protected: const char *_name; /**< driver name */ bool _debug_enabled; /**< if true, debug messages are printed */ @@ -85,7 +136,6 @@ protected: */ Device(const char *name, int irq = 0); - virtual ~Device(); /** * Initialise the driver and make it ready for use. @@ -282,48 +332,12 @@ public: * Test whether the device is currently open. * * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. * * @return True if the device is currently open. */ bool is_open() { return _open_count > 0; } - /* - * Direct access methods. - */ - - /** - * Read directly from the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param offset The device offset at which to start reading - * @param data The buffer into which the read values should be placed. - * @param count The number of items to read, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int read(unsigned offset, void *data, unsigned count = 1); - - /** - * Write directly to the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param address The device address at which to start writing. - * @param data The buffer from which values should be read. - * @param count The number of registers to write, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int write(unsigned address, void *data, unsigned count = 1); - - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform - * @param arg An argument to the operation. - * @return < 0 on error - */ - virtual int ioctl(unsigned operation, unsigned &arg); - protected: /** * Pointer to the default cdev file operations table; useful for diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index b4a9cdd53..8c4fab4c3 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev public: - /** - * Get the address - */ - uint16_t get_address() { - return _address; - } - + /** + * Get the address + */ + int16_t get_address() { return _address; } + protected: /** * The number of times a read or write operation will be retried on -- cgit v1.2.3 From 4f0ae1cdeac75111e232001e86e9d0dc2f531935 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 21:00:27 -0700 Subject: Build the px4io interfaces on top of the Device direct-access API. --- src/drivers/px4io/px4io.cpp | 37 +++++----- src/drivers/px4io/px4io_i2c.cpp | 94 +++++++++--------------- src/drivers/px4io/px4io_serial.cpp | 147 +++++++++++++++++++++---------------- 3 files changed, 134 insertions(+), 144 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 29624018f..1b4f20de0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -85,7 +85,9 @@ #include #include "uploader.h" -#include "interface.h" + +extern device::Device *PX4IO_i2c_interface() weak_function; +extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) @@ -93,7 +95,7 @@ class PX4IO : public device::CDev { public: - PX4IO(PX4IO_interface *interface); + PX4IO(device::Device *interface); virtual ~PX4IO(); virtual int init(); @@ -131,7 +133,7 @@ public: void print_status(); private: - PX4IO_interface *_interface; + device::Device *_interface; // XXX unsigned _hardware; @@ -316,7 +318,7 @@ PX4IO *g_dev; } -PX4IO::PX4IO(PX4IO_interface *interface) : +PX4IO::PX4IO(device::Device *interface) : CDev("px4io", "/dev/px4io"), _interface(interface), _hardware(0), @@ -1129,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - int ret = _interface->set_reg(page, offset, values, num_values); + int ret = _interface->write((page << 8) | offset, (void *)values, num_values); if (ret != OK) debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); return ret; @@ -1150,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } - int ret = _interface->get_reg(page, offset, values, num_values); + int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); if (ret != OK) debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); return ret; @@ -1602,12 +1604,12 @@ start(int argc, char *argv[]) if (g_dev != nullptr) errx(1, "already loaded"); - PX4IO_interface *interface; + device::Device *interface; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = io_serial_interface(); + interface = PX4IO_serial_interface(); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); + interface = PX4IO_i2c_interface(); #else # error Unknown board - cannot select interface. #endif @@ -1615,7 +1617,7 @@ start(int argc, char *argv[]) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (!interface->ok()) { + if (interface->probe()) { delete interface; errx(1, "interface init failed"); } @@ -1739,12 +1741,12 @@ monitor(void) void if_test(unsigned mode) { - PX4IO_interface *interface; + device::Device *interface; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = io_serial_interface(); + interface = PX4IO_serial_interface(); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = io_i2c_interface(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); + interface = PX4IO_i2c_interface(); #else # error Unknown board - cannot select interface. #endif @@ -1752,20 +1754,17 @@ if_test(unsigned mode) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (!interface->ok()) { + if (interface->probe()) { delete interface; errx(1, "interface init failed"); } else { - - int result = interface->test(mode); + int result = interface->ioctl(1, mode); /* XXX magic numbers */ delete interface; errx(0, "test returned %d", result); } - - exit(0); } -} +} /* namespace */ int px4io_main(int argc, char *argv[]) diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 45a707883..317326e40 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -52,109 +52,93 @@ #include +#include #include #include "uploader.h" #include -#include "interface.h" +device::Device *PX4IO_i2c_interface(); -class PX4IO_I2C : public PX4IO_interface +class PX4IO_I2C : public device::I2C { public: PX4IO_I2C(int bus, uint8_t address); virtual ~PX4IO_I2C(); - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); - - virtual bool ok(); - - virtual int test(unsigned mode); + virtual int probe(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); private: - static const unsigned _retries = 2; - struct i2c_dev_s *_dev; - uint8_t _address; }; -PX4IO_interface *io_i2c_interface(int bus, uint8_t address) +device::Device +*PX4IO_i2c_interface() { - return new PX4IO_I2C(bus, address); +#ifdef PX4_I2C_OBDEV_PX4IO + return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +#endif + return nullptr; } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - _dev(nullptr), - _address(address) + I2C("PX4IO_i2c", nullptr, bus, address, 400000) { - _dev = up_i2cinitialize(bus); - if (_dev) - I2C_SETFREQUENCY(_dev, 400000); + _retries = 3; } PX4IO_I2C::~PX4IO_I2C() { - if (_dev) - up_i2cuninitialize(_dev); } -bool -PX4IO_I2C::ok() +int +PX4IO_I2C::probe() { - if (!_dev) - return false; - - /* check any other status here */ + /* XXX really should do something here */ - return true; + return 0; } int -PX4IO_I2C::test(unsigned mode) +PX4IO_I2C::ioctl(unsigned operation, unsigned &arg) { return 0; } int -PX4IO_I2C::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +PX4IO_I2C::write(unsigned address, void *data, unsigned count) { - int ret; + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast(data); /* set up the transfer */ uint8_t addr[2] = { page, offset }; + i2c_msg_s msgv[2]; - msgv[0].addr = _address; msgv[0].flags = 0; msgv[0].buffer = addr; msgv[0].length = 2; - msgv[1].addr = _address; msgv[1].flags = I2C_M_NORESTART; msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - unsigned tries = 0; - do { + msgv[1].length = 2 * count; - /* perform the transfer */ - ret = I2C_TRANSFER(_dev, msgv, 2); - - if (ret == OK) - break; - - } while (tries++ < _retries); - - return ret; + return transfer(msgv, 2); } int -PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +PX4IO_I2C::read(unsigned address, void *data, unsigned count) { - int ret; + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast(data); /* set up the transfer */ uint8_t addr[2] = { @@ -163,25 +147,13 @@ PX4IO_I2C::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_ }; i2c_msg_s msgv[2]; - msgv[0].addr = _address; msgv[0].flags = 0; msgv[0].buffer = addr; msgv[0].length = 2; - msgv[1].addr = _address; msgv[1].flags = I2C_M_READ; msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); - - unsigned tries = 0; - do { - /* perform the transfer */ - ret = I2C_TRANSFER(_dev, msgv, 2); - - if (ret == OK) - break; - - } while (tries++ < _retries); + msgv[1].length = 2 * count; - return ret; + return transfer(msgv, 2); } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 06b955971..c48b6ec4c 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -59,6 +59,7 @@ #include +#include #include #include /* XXX should really not be hardcoding v2 here */ @@ -66,7 +67,7 @@ #include -#include "interface.h" +device::Device *PX4IO_serial_interface(); /* serial register accessors */ #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) @@ -78,17 +79,16 @@ #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -class PX4IO_serial : public PX4IO_interface +class PX4IO_serial : public device::Device { public: PX4IO_serial(); virtual ~PX4IO_serial(); - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); - - virtual bool ok(); - virtual int test(unsigned mode); + virtual int probe(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); private: /* @@ -159,12 +159,14 @@ private: IOPacket PX4IO_serial::_dma_buffer; static PX4IO_serial *g_interface; -PX4IO_interface *io_serial_interface() +device::Device +*PX4IO_serial_interface() { return new PX4IO_serial(); } PX4IO_serial::PX4IO_serial() : + Device("PX4IO_serial"), _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), @@ -262,74 +264,87 @@ PX4IO_serial::~PX4IO_serial() g_interface = nullptr; } -bool -PX4IO_serial::ok() +int +PX4IO_serial::probe() { + /* XXX this could try talking to IO */ + if (_tx_dma == nullptr) - return false; + return -1; if (_rx_dma == nullptr) - return false; + return -1; - return true; + return 0; } int -PX4IO_serial::test(unsigned mode) +PX4IO_serial::ioctl(unsigned operation, unsigned &arg) { - switch (mode) { - case 0: - lowsyslog("test 0\n"); + switch (operation) { - /* kill DMA, this is a PIO test */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); - rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); + case 1: /* XXX magic number - test operation */ + switch (arg) { + case 0: + lowsyslog("test 0\n"); - for (;;) { - while (!(rSR & USART_SR_TXE)) - ; - rDR = 0x55; - } - return 0; - - case 1: - { - unsigned fails = 0; - for (unsigned count = 0;; count++) { - uint16_t value = count & 0xffff; - - if (set_reg(PX4IO_PAGE_TEST, PX4IO_P_TEST_LED, &value, 1) != 0) - fails++; - - if (count >= 5000) { - lowsyslog("==== test 1 : %u failures ====\n", fails); - perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); - perf_print_counter(_pc_retries); - perf_print_counter(_pc_timeouts); - perf_print_counter(_pc_crcerrs); - perf_print_counter(_pc_dmaerrs); - perf_print_counter(_pc_protoerrs); - perf_print_counter(_pc_uerrs); - perf_print_counter(_pc_idle); - perf_print_counter(_pc_badidle); - count = 0; + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); + + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0x55; + } + return 0; + + case 1: + { + unsigned fails = 0; + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; + + if (count >= 5000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + count = 0; + } } + return 0; } + case 2: + lowsyslog("test 2\n"); return 0; } - case 2: - lowsyslog("test 2\n"); - return 0; + default: + break; } + return -1; } int -PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +PX4IO_serial::write(unsigned address, void *data, unsigned count) { - if (num_values > PKT_MAX_REGS) { + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast(data); + + if (count > PKT_MAX_REGS) { errno = EINVAL; return -1; } @@ -339,11 +354,11 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi int result; for (unsigned retries = 0; retries < 3; retries++) { - _dma_buffer.count_code = num_values | PKT_CODE_WRITE; + _dma_buffer.count_code = count | PKT_CODE_WRITE; _dma_buffer.page = page; _dma_buffer.offset = offset; - memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * num_values)); - for (unsigned i = num_values; i < PKT_MAX_REGS; i++) + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count)); + for (unsigned i = count; i < PKT_MAX_REGS; i++) _dma_buffer.regs[i] = 0x55aa; /* XXX implement check byte */ @@ -373,9 +388,13 @@ PX4IO_serial::set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsi } int -PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +PX4IO_serial::read(unsigned address, void *data, unsigned count) { - if (num_values > PKT_MAX_REGS) + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + uint16_t *values = reinterpret_cast(data); + + if (count > PKT_MAX_REGS) return -EINVAL; sem_wait(&_bus_semaphore); @@ -383,7 +402,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n int result; for (unsigned retries = 0; retries < 3; retries++) { - _dma_buffer.count_code = num_values | PKT_CODE_READ; + _dma_buffer.count_code = count | PKT_CODE_READ; _dma_buffer.page = page; _dma_buffer.offset = offset; @@ -402,7 +421,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n perf_count(_pc_protoerrs); /* compare the received count with the expected count */ - } else if (PKT_COUNT(_dma_buffer) != num_values) { + } else if (PKT_COUNT(_dma_buffer) != count) { /* IO returned the wrong number of registers - no point retrying */ errno = EIO; @@ -413,14 +432,14 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n } else { /* copy back the result */ - memcpy(values, &_dma_buffer.regs[0], (2 * num_values)); + memcpy(values, &_dma_buffer.regs[0], (2 * count)); } break; } perf_count(_pc_retries); } -out: + sem_post(&_bus_semaphore); return result; } -- cgit v1.2.3 From 28f996b026f4de7e572f0676834ac5eafa5dee7f Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 13 Jul 2013 21:34:31 -0700 Subject: rename the px4io serial perf counters so it's clearer what they belong to --- src/drivers/px4io/px4io_serial.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index c48b6ec4c..20b89accb 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -170,16 +170,16 @@ PX4IO_serial::PX4IO_serial() : _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), - _pc_txns(perf_alloc(PC_ELAPSED, "txns ")), - _pc_dmasetup(perf_alloc(PC_ELAPSED, "dmasetup ")), - _pc_retries(perf_alloc(PC_COUNT, "retries ")), - _pc_timeouts(perf_alloc(PC_COUNT, "timeouts ")), - _pc_crcerrs(perf_alloc(PC_COUNT, "crcerrs ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "dmaerrs ")), - _pc_protoerrs(perf_alloc(PC_COUNT, "protoerrs")), - _pc_uerrs(perf_alloc(PC_COUNT, "uarterrs ")), - _pc_idle(perf_alloc(PC_COUNT, "idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "badidle ")) + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), + _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), + _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) { /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); -- cgit v1.2.3 From 7c83e928a5ac190631047ef5b1758f1ca6b01871 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:42:56 -0700 Subject: destructors for I2C and SPI should be virtual. --- src/drivers/device/i2c.h | 2 +- src/drivers/device/spi.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 8c4fab4c3..549879352 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -88,7 +88,7 @@ protected: uint16_t address, uint32_t frequency, int irq = 0); - ~I2C(); + virtual ~I2C(); virtual int init(); diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index d2d01efb3..e0122372a 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -71,7 +71,7 @@ protected: enum spi_mode_e mode, uint32_t frequency, int irq = 0); - ~SPI(); + virtual ~SPI(); virtual int init(); -- cgit v1.2.3 From 12b84597d8058412002de6292d5def559b19c7e6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:43:43 -0700 Subject: Direct access functions return errors directly, not touching errno. --- src/drivers/device/device.cpp | 15 ++----- src/drivers/device/device.h | 93 ++++++++++++++++++++----------------------- 2 files changed, 46 insertions(+), 62 deletions(-) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index dd8074460..1c6f9b7f4 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -223,31 +223,22 @@ interrupt(int irq, void *context) return OK; } -int -Device::probe() -{ - return -1; -} - int Device::read(unsigned offset, void *data, unsigned count) { - errno = ENODEV; - return -1; + return -ENODEV; } int Device::write(unsigned offset, void *data, unsigned count) { - errno = ENODEV; - return -1; + return -ENODEV; } int Device::ioctl(unsigned operation, unsigned &arg) { - errno = ENODEV; - return -1; + return -ENODEV; } } // namespace device \ No newline at end of file diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d69d1b2d6..75f35ff0f 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -80,49 +80,49 @@ public: */ virtual void interrupt(void *ctx); /**< interrupt handler */ - /* - * Direct access methods. - */ - - /** - * Probe to test whether the device is present. - * - * @return Zero if present, < 0 (error) otherwise. - */ - virtual int probe(); - - /** - * Read directly from the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param offset The device address at which to start reading - * @param data The buffer into which the read values should be placed. - * @param count The number of items to read, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int read(unsigned address, void *data, unsigned count = 1); - - /** - * Write directly to the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param address The device address at which to start writing. - * @param data The buffer from which values should be read. - * @param count The number of registers to write, defaults to 1. - * @return count on success, < 0 on error. - */ - virtual int write(unsigned address, void *data, unsigned count = 1); - - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform - * @param arg An argument to the operation. - * @return < 0 on error - */ - virtual int ioctl(unsigned operation, unsigned &arg); + /* + * Direct access methods. + */ + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + virtual int read(unsigned address, void *data, unsigned count); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + virtual int write(unsigned address, void *data, unsigned count); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int ioctl(unsigned operation, unsigned &arg); protected: const char *_name; /**< driver name */ @@ -137,13 +137,6 @@ protected: Device(const char *name, int irq = 0); - /** - * Initialise the driver and make it ready for use. - * - * @return OK if the driver initialised OK. - */ - virtual int init(); - /** * Enable the device interrupt */ -- cgit v1.2.3 From 6c7f1e883e0e0e8f09618ba1a80075f39faadf0b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:44:46 -0700 Subject: Direct-access device functions return errors directly. Move to using ::init rather than ::probe in keeping with device changes. --- src/drivers/px4io/interface.h | 85 ---------------------------- src/drivers/px4io/px4io.cpp | 12 ++-- src/drivers/px4io/px4io_i2c.cpp | 25 ++++++-- src/drivers/px4io/px4io_serial.cpp | 113 ++++++++++++++++++------------------- 4 files changed, 79 insertions(+), 156 deletions(-) delete mode 100644 src/drivers/px4io/interface.h diff --git a/src/drivers/px4io/interface.h b/src/drivers/px4io/interface.h deleted file mode 100644 index cb7da1081..000000000 --- a/src/drivers/px4io/interface.h +++ /dev/null @@ -1,85 +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 interface.h - * - * PX4IO interface classes. - */ - -#include - -#include - -class PX4IO_interface -{ -public: - /** - * Check that the interface initialised OK. - * - * Does not check that communication has been established. - */ - virtual bool ok() = 0; - - /** - * Set PX4IO registers. - * - * @param page The register page to write - * @param offset Offset of the first register to write - * @param values Pointer to values to write - * @param num_values The number of values to write - * @return Zero on success. - */ - virtual int set_reg(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) = 0; - - /** - * Get PX4IO registers. - * - * @param page The register page to read - * @param offset Offset of the first register to read - * @param values Pointer to store values read - * @param num_values The number of values to read - * @return Zero on success. - */ - virtual int get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) = 0; - - /** - * Perform an interface test. - * - * @param mode Optional test mode. - */ - virtual int test(unsigned mode) = 0; -}; - -extern PX4IO_interface *io_i2c_interface(int bus, uint8_t address); -extern PX4IO_interface *io_serial_interface(); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1b4f20de0..904da84c4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1132,8 +1132,8 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != OK) - debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno); + if (ret != num_values) + debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return ret; } @@ -1153,8 +1153,8 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); - if (ret != OK) - debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno); + if (ret != num_values) + debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return ret; } @@ -1617,7 +1617,7 @@ start(int argc, char *argv[]) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (interface->probe()) { + if (interface->init()) { delete interface; errx(1, "interface init failed"); } @@ -1754,7 +1754,7 @@ if_test(unsigned mode) if (interface == nullptr) errx(1, "cannot alloc interface"); - if (interface->probe()) { + if (interface->init()) { delete interface; errx(1, "interface init failed"); } else { diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 317326e40..585b995fe 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file interface_i2c.cpp + * @file px4io_i2c.cpp * * I2C interface for PX4IO */ @@ -65,7 +65,7 @@ public: PX4IO_I2C(int bus, uint8_t address); virtual ~PX4IO_I2C(); - virtual int probe(); + virtual int init(); virtual int read(unsigned offset, void *data, unsigned count = 1); virtual int write(unsigned address, void *data, unsigned count = 1); virtual int ioctl(unsigned operation, unsigned &arg); @@ -94,10 +94,17 @@ PX4IO_I2C::~PX4IO_I2C() } int -PX4IO_I2C::probe() +PX4IO_I2C::init() { - /* XXX really should do something here */ + int ret; + ret = I2C::init(); + if (ret != OK) + goto out; + + /* XXX really should do something more here */ + +out: return 0; } @@ -130,7 +137,10 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count) msgv[1].buffer = (uint8_t *)values; msgv[1].length = 2 * count; - return transfer(msgv, 2); + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; } int @@ -155,5 +165,8 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) msgv[1].buffer = (uint8_t *)values; msgv[1].length = 2 * count; - return transfer(msgv, 2); + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 20b89accb..6a0b4d33f 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file interface_serial.cpp + * @file px4io_serial.cpp * * Serial interface for PX4IO */ @@ -85,7 +85,7 @@ public: PX4IO_serial(); virtual ~PX4IO_serial(); - virtual int probe(); + virtual int init(); virtual int read(unsigned offset, void *data, unsigned count = 1); virtual int write(unsigned address, void *data, unsigned count = 1); virtual int ioctl(unsigned operation, unsigned &arg); @@ -181,43 +181,6 @@ PX4IO_serial::PX4IO_serial() : _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) { - /* allocate DMA */ - _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); - _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); - if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) - return; - - /* configure pins for serial use */ - stm32_configgpio(PX4IO_SERIAL_TX_GPIO); - stm32_configgpio(PX4IO_SERIAL_RX_GPIO); - - /* reset & configure the UART */ - rCR1 = 0; - rCR2 = 0; - rCR3 = 0; - - /* eat any existing interrupt status */ - (void)rSR; - (void)rDR; - - /* configure line speed */ - uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); - uint32_t mantissa = usartdiv32 >> 5; - uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; - rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); - - /* attach serial interrupt handler */ - irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); - up_enable_irq(PX4IO_SERIAL_VECTOR); - - /* enable UART in DMA mode, enable error and line idle interrupts */ - /* rCR3 = USART_CR3_EIE;*/ - rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; - - /* create semaphores */ - sem_init(&_completion_semaphore, 0, 0); - sem_init(&_bus_semaphore, 0, 1); - g_interface = this; } @@ -265,15 +228,48 @@ PX4IO_serial::~PX4IO_serial() } int -PX4IO_serial::probe() +PX4IO_serial::init() { - /* XXX this could try talking to IO */ - - if (_tx_dma == nullptr) - return -1; - if (_rx_dma == nullptr) + /* allocate DMA */ + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return -1; + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); + + /* reset & configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* enable UART in DMA mode, enable error and line idle interrupts */ + /* rCR3 = USART_CR3_EIE;*/ + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + + /* create semaphores */ + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); + + + /* XXX this could try talking to IO */ + return 0; } @@ -344,10 +340,8 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) uint8_t offset = address & 0xff; const uint16_t *values = reinterpret_cast(data); - if (count > PKT_MAX_REGS) { - errno = EINVAL; - return -1; - } + if (count > PKT_MAX_REGS) + return -EINVAL; sem_wait(&_bus_semaphore); @@ -373,8 +367,7 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { /* IO didn't like it - no point retrying */ - errno = EINVAL; - result = -1; + result = -EINVAL; perf_count(_pc_protoerrs); } @@ -384,6 +377,9 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count) } sem_post(&_bus_semaphore); + + if (result == OK) + result = count; return result; } @@ -416,16 +412,14 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { /* IO didn't like it - no point retrying */ - errno = EINVAL; - result = -1; + result = -EINVAL; perf_count(_pc_protoerrs); /* compare the received count with the expected count */ } else if (PKT_COUNT(_dma_buffer) != count) { /* IO returned the wrong number of registers - no point retrying */ - errno = EIO; - result = -1; + result = -EIO; perf_count(_pc_protoerrs); /* successful read */ @@ -441,6 +435,9 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count) } sem_post(&_bus_semaphore); + + if (result == OK) + result = count; return result; } @@ -524,8 +521,7 @@ PX4IO_serial::_wait_complete() /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { perf_count(_pc_dmaerrs); - ret = -1; - errno = EIO; + ret = -EIO; break; } @@ -534,8 +530,7 @@ PX4IO_serial::_wait_complete() _dma_buffer.crc = 0; if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); - ret = -1; - errno = EIO; + ret = -EIO; break; } -- cgit v1.2.3 From 5350c2be09af7eb88233cb89f8c013974a586e53 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:45:21 -0700 Subject: Refactor MS5611 driver to use interface nubs for the I2C and SPI versions of the chip. This reduces the amount of duplicated code. --- src/drivers/ms5611/ms5611.cpp | 1435 ++++++------------------------------- src/drivers/ms5611/ms5611.h | 86 +++ src/drivers/ms5611/ms5611_i2c.cpp | 264 +++++++ src/drivers/ms5611/ms5611_spi.cpp | 228 ++++++ 4 files changed, 782 insertions(+), 1231 deletions(-) create mode 100644 src/drivers/ms5611/ms5611.h create mode 100644 src/drivers/ms5611/ms5611_i2c.cpp create mode 100644 src/drivers/ms5611/ms5611_spi.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 76acf7ccd..b8c396f7b 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -38,9 +38,6 @@ #include -#include -#include - #include #include #include @@ -60,12 +57,14 @@ #include +#include +#include #include #include #include -#include +#include "ms5611.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -77,176 +76,25 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) -/** - * Calibration PROM as reported by the device. - */ -#pragma pack(push,1) -struct ms5611_prom_s { - uint16_t factory_setup; - uint16_t c1_pressure_sens; - uint16_t c2_pressure_offset; - uint16_t c3_temp_coeff_pres_sens; - uint16_t c4_temp_coeff_pres_offset; - uint16_t c5_reference_temp; - uint16_t c6_temp_coeff_temp; - uint16_t serial_and_crc; -}; +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) -/** - * Grody hack for crc4() +/* + * MS5611 internal constants and data structures. */ -union ms5611_prom_u { - uint16_t c[8]; - struct ms5611_prom_s s; -}; -#pragma pack(pop) - -class MS5611_I2C : public device::I2C -{ -public: - MS5611_I2C(int bus); - virtual ~MS5611_I2C(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - union ms5611_prom_u _prom; - - struct work_s _work; - unsigned _measure_ticks; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; - - bool _collect_phase; - unsigned _measure_phase; - - /* intermediate temperature values per MS5611 datasheet */ - int32_t _TEMP; - int64_t _OFF; - int64_t _SENS; - - /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ - - orb_advert_t _baro_topic; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - /** - * Initialize the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start_cycle(); - - /** - * Stop the automatic measurement state machine. - */ - void stop_cycle(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * Issue a measurement command for the current state. - * - * @return OK if the measurement command was successful. - */ - virtual int measure(); - - /** - * Collect the result of the most recent measurement. - */ - virtual int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - virtual int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - virtual int read_prom(); - - /** - * Execute the bus-specific ioctl (I2C or SPI) - * - * @return the bus-specific ioctl return value - */ - virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - -private: - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - -}; +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -class MS5611_SPI : public device::SPI +class MS5611 : public device::CDev { public: - MS5611_SPI(int bus, const char* path, spi_dev_e device); - virtual ~MS5611_SPI(); + MS5611(device::Device *interface); + ~MS5611(); virtual int init(); @@ -259,8 +107,9 @@ public: void print_info(); protected: - virtual int probe(); - union ms5611_prom_u _prom; + Device *_interface; + + ms5611::prom_u _prom; struct work_s _work; unsigned _measure_ticks; @@ -335,116 +184,16 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - virtual int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - virtual int read_prom(); - - /** - * Execute the bus-specific ioctl (I2C or SPI) - * - * @return the bus-specific ioctl return value - */ - virtual int bus_ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - - // XXX this should really go into the SPI base class, as its common code - uint8_t read_reg(unsigned reg); - uint16_t read_reg16(unsigned reg); - void write_reg(unsigned reg, uint8_t value); - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - -private: - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MS5611 internal constants and data structures. - */ - -/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ - -#ifndef PX4_I2C_BUS_ONBOARD - #define MS5611_BUS 1 -#else - #define MS5611_BUS PX4_I2C_BUS_ONBOARD -#endif - -#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ - -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ - /* * Driver 'main' command. */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611_I2C::MS5611_I2C(int bus) : - I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), - _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _collect_phase(false), - _measure_phase(0), - _TEMP(0), - _OFF(0), - _SENS(0), - _msl_pressure(101325), - _baro_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), - _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) -{ - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - warnx("MS5611 I2C constructor"); -} - -MS5611_SPI::MS5611_SPI(int bus, const char* path, spi_dev_e device) : - SPI("MS5611", path, bus, device, SPIDEV_MODE3, 2000000), +MS5611::MS5611(device::Device *interface) : + CDev("MS5611", BARO_DEVICE_PATH), + _interface(interface), _measure_ticks(0), _num_reports(0), _next_report(0), @@ -462,12 +211,11 @@ MS5611_SPI::MS5611_SPI(int bus, const char* path, spi_dev_e device) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in the dtor will explode if we don't do this... + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); - warnx("MS5611 SPI constructor"); } -MS5611_I2C::~MS5611_I2C() +MS5611::~MS5611() { /* make sure we are truly inactive */ stop_cycle(); @@ -475,33 +223,32 @@ MS5611_I2C::~MS5611_I2C() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; -} - -MS5611_SPI::~MS5611_SPI() -{ - /* make sure we are truly inactive */ - stop_cycle(); - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; + delete _interface; } int -MS5611_I2C::init() +MS5611::init() { - int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + /* verify that the interface is ok */ + unsigned arg = (unsigned)&_prom; + _interface->ioctl(IOCTL_SET_PROMBUFFER, arg); + int ret = _interface->init(); + if (ret != OK) { + debug("interface init failed"); goto out; + } /* allocate basic report buffers */ _num_reports = 2; _reports = new struct baro_report[_num_reports]; - if (_reports == nullptr) + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; goto out; + } _oldest_report = _next_report = 0; @@ -509,104 +256,19 @@ MS5611_I2C::init() memset(&_reports[0], 0, sizeof(_reports[0])); _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); - if (_baro_topic < 0) + if (_baro_topic < 0) { debug("failed to create sensor_baro object"); - - ret = OK; -out: - return ret; -} - -int -MS5611_I2C::probe() -{ -#ifdef PX4_I2C_OBDEV_MS5611 - _retries = 10; - - if ((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return OK; - } -#endif - - return -EIO; -} - -int -MS5611_SPI::init() -{ - int ret = ERROR; - - /* do SPI init (and probe) first */ - warnx("MS5611 SPI init function"); - if (SPI::init() != OK) { - warnx("SPI init failed"); + ret = -ENOSPC; goto out; - } else { - warnx("SPI init ok"); } - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; - - if (_reports == nullptr) - goto out; - - _oldest_report = _next_report = 0; - - /* get a publish handle on the baro topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); - - if (_baro_topic < 0) - debug("failed to create sensor_baro object"); - ret = OK; out: return ret; } -int -MS5611_SPI::probe() -{ - - warnx("SPI probe"); - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - -int -MS5611_I2C::probe_address(uint8_t address) -{ - /* select the address we are going to try */ - set_address(address); - - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - ssize_t -MS5611_I2C::read(struct file *filp, char *buffer, size_t buflen) +MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); int ret = 0; @@ -676,214 +338,10 @@ MS5611_I2C::read(struct file *filp, char *buffer, size_t buflen) return ret; } -ssize_t -MS5611_SPI::read(struct file *filp, char *buffer, size_t buflen) +int +MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) { - unsigned count = buflen / sizeof(struct baro_report); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); - - return ret; -} - -int -MS5611_SPI::bus_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); -} - -int -MS5611_I2C::bus_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); -} - -int -MS5611_I2C::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; - return OK; - - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - case BAROIOCSMSLPRESSURE: - - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) - return -EINVAL; - - _msl_pressure = arg; - return OK; - - case BAROIOCGMSLPRESSURE: - return _msl_pressure; - - default: - break; - } - - /* give it to the bus-specific superclass */ - // return bus_ioctl(filp, cmd, arg); - return I2C::ioctl(filp, cmd, arg); -} - -int -MS5611_SPI::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { + switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { @@ -941,512 +399,160 @@ MS5611_SPI::ioctl(struct file *filp, int cmd, unsigned long arg) } } - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - case BAROIOCSMSLPRESSURE: - - /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) - return -EINVAL; - - _msl_pressure = arg; - return OK; - - case BAROIOCGMSLPRESSURE: - return _msl_pressure; - - default: - break; - } - - /* give it to the bus-specific superclass */ - // return bus_ioctl(filp, cmd, arg); - return SPI::ioctl(filp, cmd, arg); -} - -void -MS5611_I2C::start_cycle() -{ - - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611_I2C::cycle_trampoline, this, 1); -} - -void -MS5611_I2C::stop_cycle() -{ - work_cancel(HPWORK, &_work); -} - -void -MS5611_I2C::cycle_trampoline(void *arg) -{ - MS5611_I2C *dev = (MS5611_I2C *)arg; - - dev->cycle(); -} - -void -MS5611_I2C::cycle() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with the message. - */ - } else { - //log("collection error %d", ret); - } - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611_I2C::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - ret = measure(); - if (ret != OK) { - //log("measure error %d", ret); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611_I2C::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); -} - -void -MS5611_SPI::start_cycle() -{ - - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611_SPI::cycle_trampoline, this, 1); -} - -void -MS5611_SPI::stop_cycle() -{ - work_cancel(HPWORK, &_work); -} - -void -MS5611_SPI::cycle_trampoline(void *arg) -{ - MS5611_SPI *dev = (MS5611_SPI *)arg; - - dev->cycle(); -} - -void -MS5611_SPI::cycle() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - if (ret != OK) { - if (ret == -6) { - /* - * The ms5611 seems to regularly fail to respond to - * its address; this happens often enough that we'd rather not - * spam the console with the message. - */ - } else { - //log("collection error %d", ret); - } - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - * Don't inject one after temperature measurements, so we can keep - * doing pressure measurements at something close to the desired rate. - */ - if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611_SPI::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - ret = measure(); - if (ret != OK) { - //log("measure error %d", ret); - /* reset the collection state machine and try again */ - start_cycle(); - return; - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611_SPI::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); -} - -int -MS5611_I2C::measure() -{ - int ret; - - perf_begin(_measure_perf); - - /* - * In phase zero, request temperature; in other phases, request pressure. - */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; - - /* - * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. - */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); - - if (OK != ret) - perf_count(_comms_errors); - - perf_end(_measure_perf); - - return ret; -} - -int -MS5611_I2C::collect() -{ - int ret; - uint8_t cmd; - uint8_t data[3]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - cmd = 0; - - perf_begin(_sample_perf); - - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - - ret = transfer(&cmd, 1, &data[0], 3); - if (ret != OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - - /* handle a measurement */ - if (_measure_phase == 0) { - - /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); - - /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); - - /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); - - /* temperature compensation */ - if (_TEMP < 2000) { - - int32_t T2 = POW2(dT) >> 31; - - int64_t f = POW2((int64_t)_TEMP - 2000); - int64_t OFF2 = 5 * f >> 1; - int64_t SENS2 = 5 * f >> 2; - - if (_TEMP < -1500) { - int64_t f2 = POW2(_TEMP + 1500); - OFF2 += 7 * f2; - SENS2 += 11 * f2 >> 1; - } - - _TEMP -= T2; - _OFF -= OFF2; - _SENS -= SENS2; - } - - } else { - - /* pressure calculation, result in Pa */ - int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - - /* generate a new report */ - _reports[_next_report].temperature = _TEMP / 100.0f; - _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ - - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ - - /* - * PERFORMANCE HINT: - * - * The single precision calculation is 50 microseconds faster than the double - * precision variant. It is however not obvious if double precision is required. - * Pending more inspection and tests, we'll leave the double precision variant active. - * - * Measurements: - * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us - * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us - */ - - /* tropospheric properties (0-11km) for standard atmosphere */ - const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ - const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ - const double g = 9.80665; /* gravity constant in m/s/s */ - const double R = 287.05; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - double p1 = _msl_pressure / 1000.0; - - /* measured pressure in kPa */ - double p = P / 1000.0; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - } - - /* update the measurement state machine */ - INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; - perf_end(_sample_perf); + return (1000 / _measure_ticks); - return OK; -} + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; -int -MS5611_I2C::cmd_reset() -{ - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; - return result; -} + if (nullptr == buf) + return -ENOMEM; -int -MS5611_I2C::read_prom() -{ - uint8_t prom_buf[2]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; + /* reset the measurement state machine with the new buffer, free the old */ + stop_cycle(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start_cycle(); - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); + return OK; + } - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; - } + case BAROIOCSMSLPRESSURE: - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; -} + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; -uint8_t -MS5611_SPI::read_reg(unsigned reg) -{ - uint8_t cmd[2]; + _msl_pressure = arg; + return OK; - cmd[0] = reg | DIR_READ; + case BAROIOCGMSLPRESSURE: + return _msl_pressure; - transfer(cmd, cmd, sizeof(cmd)); + default: + break; + } - return cmd[1]; + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } -uint16_t -MS5611_SPI::read_reg16(unsigned reg) +void +MS5611::start_cycle() { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; - return (uint16_t)(cmd[1] << 8) | cmd[2]; + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); } void -MS5611_SPI::write_reg(unsigned reg, uint8_t value) +MS5611::stop_cycle() { - uint8_t cmd[2]; + work_cancel(HPWORK, &_work); +} - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; +void +MS5611::cycle_trampoline(void *arg) +{ + MS5611 *dev = reinterpret_cast(arg); - transfer(cmd, nullptr, sizeof(cmd)); + dev->cycle(); } void -MS5611_SPI::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +MS5611::cycle() { - uint8_t val; + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with a message for this. + */ + } else { + //log("collection error %d", ret); + } + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int -MS5611_SPI::measure() +MS5611::measure() { int ret; @@ -1455,17 +561,12 @@ MS5611_SPI::measure() /* * In phase zero, request temperature; in other phases, request pressure. */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; - cmd_data |= DIR_WRITE; + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; /* * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. */ - ret = transfer(&cmd_data, nullptr, 1); - + ret = _interface->ioctl(IOCTL_MEASURE, addr); if (OK != ret) perf_count(_comms_errors); @@ -1475,38 +576,24 @@ MS5611_SPI::measure() } int -MS5611_SPI::collect() +MS5611::collect() { int ret; - - uint8_t data[4]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - data[0] = 0 | DIR_WRITE; + uint32_t raw; perf_begin(_sample_perf); /* this should be fairly close to the end of the conversion, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); - ret = transfer(&data[0], &data[0], sizeof(data)); - if (ret != OK) { + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->read(0, (void *)&raw, 0); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - /* fetch the raw value */ - cvt.b[0] = data[3]; - cvt.b[1] = data[2]; - cvt.b[2] = data[1]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - /* handle a measurement */ if (_measure_phase == 0) { @@ -1585,7 +672,7 @@ MS5611_SPI::collect() * a */ _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - + /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); @@ -1610,96 +697,8 @@ MS5611_SPI::collect() return OK; } -int -MS5611_SPI::cmd_reset() -{ - uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - int result; - - result = transfer(&cmd, nullptr, 1); - warnx("transferred reset, result: %d", result); - - return result; -} - -int -MS5611_SPI::read_prom() -{ - uint8_t prom_buf[3]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); - _prom.c[i] = read_reg16(cmd); - } - - warnx("going for CRC"); - - /* calculate CRC and return success/failure accordingly */ - int ret = crc4(&_prom.c[0]) ? OK : -EIO; - if (ret == OK) { - warnx("CRC OK"); - } else { - warnx("CRC FAIL"); - } - return ret; -} - -bool -MS5611_I2C::crc4(uint16_t *n_prom) -{ - int16_t cnt; - uint16_t n_rem; - uint16_t crc_read; - uint8_t n_bit; - - n_rem = 0x00; - - /* save the read crc */ - crc_read = n_prom[7]; - - /* remove CRC byte */ - n_prom[7] = (0xFF00 & (n_prom[7])); - - for (cnt = 0; cnt < 16; cnt++) { - /* uneven bytes */ - if (cnt & 1) { - n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); - - } else { - n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); - } - - for (n_bit = 8; n_bit > 0; n_bit--) { - if (n_rem & 0x8000) { - n_rem = (n_rem << 1) ^ 0x3000; - - } else { - n_rem = (n_rem << 1); - } - } - } - - /* final 4 bit remainder is CRC value */ - n_rem = (0x000F & (n_rem >> 12)); - n_prom[7] = crc_read; - - /* return true if CRCs match */ - return (0x000F & crc_read) == (n_rem ^ 0x00); -} - void -MS5611_I2C::print_info() +MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -1722,8 +721,25 @@ MS5611_I2C::print_info() printf("serial_and_crc %u\n", _prom.s.serial_and_crc); } +/** + * Local functions in support of the shell command. + */ +namespace ms5611 +{ + +MS5611 *g_dev; + +void start(); +void test(); +void reset(); +void info(); +void calibrate(unsigned altitude); + +/** + * MS5611 crc4 cribbed from the datasheet + */ bool -MS5611_SPI::crc4(uint16_t *n_prom) +crc4(uint16_t *n_prom) { int16_t cnt; uint16_t n_rem; @@ -1765,43 +781,6 @@ MS5611_SPI::crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } -void -MS5611_SPI::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); - printf("TEMP: %d\n", _TEMP); - printf("SENS: %lld\n", _SENS); - printf("OFF: %lld\n", _OFF); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); -} - -/** - * Local functions in support of the shell command. - */ -namespace ms5611 -{ - -device::CDev *g_dev; - -void start(); -void test(); -void reset(); -void info(); -void calibrate(unsigned altitude); /** * Start the driver. @@ -1814,34 +793,29 @@ start() if (g_dev != nullptr) errx(1, "already started"); - /* create the driver, try SPI first, fall back to I2C if required */ - #ifdef PX4_SPIDEV_BARO - { - warnx("Attempting SPI start"); - g_dev = new MS5611_SPI(1 /* XXX magic number, SPI1 */, BARO_DEVICE_PATH,(spi_dev_e)PX4_SPIDEV_BARO); - } - #endif - /* try I2C if configuration exists and SPI failed*/ - #ifdef MS5611_BUS - if (g_dev == nullptr) { - warnx("Attempting I2C start"); - g_dev = new MS5611_I2C(MS5611_BUS); - } + device::Device *interface = nullptr; - #endif + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ + if (MS5611_spi_interface != nullptr) + interface = MS5611_spi_interface(); + if (interface == nullptr && (MS5611_i2c_interface != nullptr)) + interface = MS5611_i2c_interface(); - if (g_dev == nullptr) - goto fail; + if (interface == nullptr) + errx(1, "failed to allocate an interface"); - if (OK != g_dev->init()) + g_dev = new MS5611(interface); + if (g_dev == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev->init() != OK) goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); - if (fd < 0) goto fail; - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; @@ -1952,8 +926,7 @@ info() errx(1, "driver not running"); printf("state @ %p\n", g_dev); - MS5611_SPI* spi = (MS5611_SPI*)g_dev; - spi->print_info(); + g_dev->print_info(); exit(0); } diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h new file mode 100644 index 000000000..872011dc9 --- /dev/null +++ b/src/drivers/ms5611/ms5611.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 ms5611.h + * + * Shared defines for the ms5611 driver. + */ + +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* interface ioctls */ +#define IOCTL_SET_PROMBUFFER 1 +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + + +/* interface factories */ +extern device::Device *MS5611_spi_interface() weak_function; +extern device::Device *MS5611_i2c_interface() weak_function; + +namespace ms5611 +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +extern bool crc4(uint16_t *n_prom); + +} /* namespace */ diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp new file mode 100644 index 000000000..7f5667c28 --- /dev/null +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * 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 ms5611_i2c.cpp + * + * I2C interface for MS5611 + */ + +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + + + +device::Device *MS5611_i2c_interface(); + +class MS5611_I2C : device::I2C +{ +public: + MS5611_I2C(int bus); + virtual ~MS5611_I2C(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + ms5611::prom_u *_prom + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +MS5611_i2c_interface() +{ +#ifdef PX4_I2C_OBDEV_MS5611 + return new MS5611_I2C(MS5611_BUS); +#endif + return nullptr; +} + +MS5611_I2C::MS5611_I2C(int bus, ms5611_prom_u &prom) : + I2C("MS5611_I2C", nullptr, bus, 0, 400000), + _prom(prom) +{ +} + +MS5611_I2C::~MS5611_I2C() +{ +} + +int +MS5611_I2C::init() +{ + /* this will call probe(), and thereby _probe_address */ + return I2C::init(); +} + +int +MS5611_I2C::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MS5611_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_SET_PROMBUFFER: + _prom = reinterpret_cast(arg); + ret = OK; + break; + + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +MS5611_I2C::probe() +{ + _retries = 10; + + if ((OK == _probe_address(MS5611_ADDRESS_1)) || + (OK == _probe_address(MS5611_ADDRESS_2))) { + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; + return OK; + } + + return -EIO; +} + +int +MS5611_I2C::_probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_address(address); + + /* send reset command */ + if (OK != _reset()) + return -EIO; + + /* read PROM */ + if (OK != _read_prom()) + return -EIO; + + return OK; +} + + +int +MS5611_I2C::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611_I2C::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +MS5611_I2C::_read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom->c[i] = cvt.w; + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom->c[0]) ? OK : -EIO; +} + diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp new file mode 100644 index 000000000..eed8e1697 --- /dev/null +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * 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 ms5611_spi.cpp + * + * SPI interface for MS5611 + */ + + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +device::Device *MS5611_spi_interface(); + +class MS5611_SPI : device::SPI +{ +public: + MS5611_SPI(int bus, spi_dev_e device); + virtual ~MS5611_SPI(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + ms5611::prom_u *_prom + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + + /** + * Read a 16-bit register value. + * + * @param reg The register to read. + */ + uint16_t _reg16(unsigned reg); +}; + +device::Device * +MS5611_spi_interface() +{ +#ifdef PX4_SPIDEV_BARO + return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO); +#endif + return nullptr; +} + +int +MS5611_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) + goto out; + + /* send reset command */ + ret = _reset(); + if (ret != OK) + goto out; + + /* read PROM */ + ret = _read_prom(); + if (ret != OK) + goto out; + +out: + return ret; +} + +int +MS5611_SPI::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[4]; + + /* read the most recent measurement */ + buf[0] = 0 | DIR_WRITE; + int ret = transfer(&buf[0], &buf[0], sizeof(buf)); + + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = data[3]; + cvt->b[1] = data[2]; + cvt->b[2] = data[1]; + cvt->b[3] = 0; + + ret = count; + } + + return ret; +} + +int +MS5611_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_SET_PROMBUFFER: + _prom = reinterpret_cast(arg); + return OK; + + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + if (ret != OK) { + errno = ret; + return -1; + } + return 0; +} + +int +MS5611_SPI::_reset() +{ + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; + + return transfer(&cmd, nullptr, 1); +} + +int +MS5611_SPI::_measure(unsigned addr) +{ + uint8_t cmd = addr | DIR_WRITE; + + return transfer(&cmd, nullptr, 1); +} + + +int +MS5611_SPI::_read_prom() +{ + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); + _prom.c[i] = _reg16(cmd); + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; +} + +uint16_t +MS5611_SPI::_reg16(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} -- cgit v1.2.3 From f8f6a43feac372c310f3d2444a8533b09e201d6c Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 11:50:35 -0700 Subject: Use common, board-type-agnostic code to allocate the PX4IO interface. --- src/drivers/px4io/px4io.cpp | 66 ++++++++++++++++++++++----------------------- 1 file changed, 32 insertions(+), 34 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 904da84c4..9f84c0950 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1598,30 +1598,43 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { -void -start(int argc, char *argv[]) +device::Device * +get_interface() { - if (g_dev != nullptr) - errx(1, "already loaded"); + device::Device *interface = nullptr; - device::Device *interface; + /* try for a serial interface */ + if (PX4IO_serial_interface != nullptr) + interface = PX4IO_serial_interface(); + if (interface != nullptr) + goto got; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = PX4IO_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = PX4IO_i2c_interface(); -#else -# error Unknown board - cannot select interface. -#endif + /* try for an I2C interface if we haven't got a serial one */ + if (PX4IO_i2c_interface != nullptr) + interface = PX4IO_i2c_interface(); + if (interface != nullptr) + goto got; - if (interface == nullptr) - errx(1, "cannot alloc interface"); + errx(1, "cannot alloc interface"); - if (interface->init()) { +got: + if (interface->init() != OK) { delete interface; errx(1, "interface init failed"); } + return interface; +} + +void +start(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(1, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + /* create the driver - it will set g_dev */ (void)new PX4IO(interface); @@ -1741,27 +1754,12 @@ monitor(void) void if_test(unsigned mode) { - device::Device *interface; + device::Device *interface = get_interface(); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - interface = PX4IO_serial_interface(); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - interface = PX4IO_i2c_interface(); -#else -# error Unknown board - cannot select interface. -#endif + int result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; - if (interface == nullptr) - errx(1, "cannot alloc interface"); - - if (interface->init()) { - delete interface; - errx(1, "interface init failed"); - } else { - int result = interface->ioctl(1, mode); /* XXX magic numbers */ - delete interface; - errx(0, "test returned %d", result); - } + errx(0, "test returned %d", result); } } /* namespace */ -- cgit v1.2.3 From b11e05d614c372c45a3492e2c3b45ab252defced Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 12:40:26 -0700 Subject: Don't build interface drivers we don't have config for. --- src/drivers/px4io/px4io_i2c.cpp | 12 ++++-------- src/drivers/px4io/px4io_serial.cpp | 4 ++++ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 585b995fe..f561b4359 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -50,12 +50,9 @@ #include -#include - #include -#include -#include "uploader.h" -#include + +#ifdef PX4_I2C_OBDEV_PX4IO device::Device *PX4IO_i2c_interface(); @@ -77,10 +74,7 @@ private: device::Device *PX4IO_i2c_interface() { -#ifdef PX4_I2C_OBDEV_PX4IO return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); -#endif - return nullptr; } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : @@ -170,3 +164,5 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) ret = count; return ret; } + +#endif /* PX4_I2C_OBDEV_PX4IO */ \ No newline at end of file diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 6a0b4d33f..840b96f5b 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -67,6 +67,8 @@ #include +#ifdef PX4IO_SERIAL_BASE + device::Device *PX4IO_serial_interface(); /* serial register accessors */ @@ -667,3 +669,5 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); } + +#endif /* PX4IO_SERIAL_BASE */ \ No newline at end of file -- cgit v1.2.3 From 6cf120831289368015b7b4f51db4f99f418e7129 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 14 Jul 2013 12:42:51 -0700 Subject: Don't build interface drivers we don't have configs for. Make the interface drivers build. Change the way we handle the prom buffer so that we can init the interface before constructing the driver. --- src/drivers/ms5611/module.mk | 2 +- src/drivers/ms5611/ms5611.cpp | 59 ++++++++++++++++++-------------- src/drivers/ms5611/ms5611.h | 11 +++--- src/drivers/ms5611/ms5611_i2c.cpp | 46 ++++++++++++++++--------- src/drivers/ms5611/ms5611_spi.cpp | 72 ++++++++++++++++++++++++++------------- 5 files changed, 117 insertions(+), 73 deletions(-) diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 3c4b0f093..20f8aa173 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = ms5611 -SRCS = ms5611.cpp +SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index b8c396f7b..9d9888a90 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; class MS5611 : public device::CDev { public: - MS5611(device::Device *interface); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf); ~MS5611(); virtual int init(); @@ -109,7 +109,7 @@ public: protected: Device *_interface; - ms5611::prom_u _prom; + ms5611::prom_s _prom; struct work_s _work; unsigned _measure_ticks; @@ -191,9 +191,10 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface) : +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), + _prom(prom_buf.s), _measure_ticks(0), _num_reports(0), _next_report(0), @@ -230,13 +231,11 @@ MS5611::~MS5611() int MS5611::init() { + int ret; - /* verify that the interface is ok */ - unsigned arg = (unsigned)&_prom; - _interface->ioctl(IOCTL_SET_PROMBUFFER, arg); - int ret = _interface->init(); + ret = CDev::init(); if (ret != OK) { - debug("interface init failed"); + debug("CDev init failed"); goto out; } @@ -598,14 +597,14 @@ MS5611::collect() if (_measure_phase == 0) { /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); /* temperature compensation */ if (_TEMP < 2000) { @@ -711,14 +710,14 @@ MS5611::print_info() printf("OFF: %lld\n", _OFF); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); } /** @@ -789,6 +788,7 @@ void start() { int fd; + prom_u prom_buf; if (g_dev != nullptr) errx(1, "already started"); @@ -797,14 +797,19 @@ start() /* create the driver, try SPI first, fall back to I2C if unsuccessful */ if (MS5611_spi_interface != nullptr) - interface = MS5611_spi_interface(); + interface = MS5611_spi_interface(prom_buf); if (interface == nullptr && (MS5611_i2c_interface != nullptr)) - interface = MS5611_i2c_interface(); + interface = MS5611_i2c_interface(prom_buf); if (interface == nullptr) errx(1, "failed to allocate an interface"); - g_dev = new MS5611(interface); + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } + + g_dev = new MS5611(interface, prom_buf); if (g_dev == nullptr) { delete interface; errx(1, "failed to allocate driver"); @@ -814,10 +819,14 @@ start() /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { + warnx("can't open baro device"); goto fail; - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warnx("failed setting default poll rate"); goto fail; + } exit(0); diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index 872011dc9..76fb84de8 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -45,15 +45,9 @@ #define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ -#define IOCTL_SET_PROMBUFFER 1 #define IOCTL_RESET 2 #define IOCTL_MEASURE 3 - -/* interface factories */ -extern device::Device *MS5611_spi_interface() weak_function; -extern device::Device *MS5611_i2c_interface() weak_function; - namespace ms5611 { @@ -84,3 +78,8 @@ union prom_u { extern bool crc4(uint16_t *n_prom); } /* namespace */ + +/* interface factories */ +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; + diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 7f5667c28..06867cc9d 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -37,6 +37,25 @@ * I2C interface for MS5611 */ +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "ms5611.h" + +#ifdef PX4_I2C_OBDEV_MS5611 + #ifndef PX4_I2C_BUS_ONBOARD #define MS5611_BUS 1 #else @@ -48,12 +67,12 @@ -device::Device *MS5611_i2c_interface(); +device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); -class MS5611_I2C : device::I2C +class MS5611_I2C : public device::I2C { public: - MS5611_I2C(int bus); + MS5611_I2C(int bus, ms5611::prom_u &prom_buf); virtual ~MS5611_I2C(); virtual int init(); @@ -64,7 +83,7 @@ protected: virtual int probe(); private: - ms5611::prom_u *_prom + ms5611::prom_u &_prom; int _probe_address(uint8_t address); @@ -92,15 +111,12 @@ private: }; device::Device * -MS5611_i2c_interface() +MS5611_i2c_interface(ms5611::prom_u &prom_buf) { -#ifdef PX4_I2C_OBDEV_MS5611 - return new MS5611_I2C(MS5611_BUS); -#endif - return nullptr; + return new MS5611_I2C(MS5611_BUS, prom_buf); } -MS5611_I2C::MS5611_I2C(int bus, ms5611_prom_u &prom) : +MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : I2C("MS5611_I2C", nullptr, bus, 0, 400000), _prom(prom) { @@ -146,11 +162,6 @@ MS5611_I2C::ioctl(unsigned operation, unsigned &arg) int ret; switch (operation) { - case IOCTL_SET_PROMBUFFER: - _prom = reinterpret_cast(arg); - ret = OK; - break; - case IOCTL_RESET: ret = _reset(); break; @@ -255,10 +266,11 @@ MS5611_I2C::_read_prom() /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ cvt.b[0] = prom_buf[1]; cvt.b[1] = prom_buf[0]; - _prom->c[i] = cvt.w; + _prom.c[i] = cvt.w; } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom->c[0]) ? OK : -EIO; + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; } +#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index eed8e1697..156832a61 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -37,31 +37,44 @@ * SPI interface for MS5611 */ +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "ms5611.h" /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -device::Device *MS5611_spi_interface(); +#ifdef PX4_SPIDEV_BARO + +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); -class MS5611_SPI : device::SPI +class MS5611_SPI : public device::SPI { public: - MS5611_SPI(int bus, spi_dev_e device); + MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); virtual ~MS5611_SPI(); virtual int init(); virtual int read(unsigned offset, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); -protected: - virtual int probe(); - private: - ms5611::prom_u *_prom - - int _probe_address(uint8_t address); + ms5611::prom_u &_prom; /** * Send a reset command to the MS5611. @@ -93,12 +106,19 @@ private: }; device::Device * -MS5611_spi_interface() +MS5611_spi_interface(ms5611::prom_u &prom_buf) +{ + return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +} + +MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + _prom(prom_buf) +{ +} + +MS5611_SPI::~MS5611_SPI() { -#ifdef PX4_SPIDEV_BARO - return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO); -#endif - return nullptr; } int @@ -107,18 +127,24 @@ MS5611_SPI::init() int ret; ret = SPI::init(); - if (ret != OK) + if (ret != OK) { + debug("SPI init failed"); goto out; + } /* send reset command */ ret = _reset(); - if (ret != OK) + if (ret != OK) { + debug("reset failed"); goto out; + } /* read PROM */ ret = _read_prom(); - if (ret != OK) + if (ret != OK) { + debug("prom readout failed"); goto out; + } out: return ret; @@ -139,9 +165,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) if (ret == OK) { /* fetch the raw value */ - cvt->b[0] = data[3]; - cvt->b[1] = data[2]; - cvt->b[2] = data[1]; + cvt->b[0] = buf[3]; + cvt->b[1] = buf[2]; + cvt->b[2] = buf[1]; cvt->b[3] = 0; ret = count; @@ -156,10 +182,6 @@ MS5611_SPI::ioctl(unsigned operation, unsigned &arg) int ret; switch (operation) { - case IOCTL_SET_PROMBUFFER: - _prom = reinterpret_cast(arg); - return OK; - case IOCTL_RESET: ret = _reset(); break; @@ -226,3 +248,5 @@ MS5611_SPI::_reg16(unsigned reg) return (uint16_t)(cmd[1] << 8) | cmd[2]; } + +#endif /* PX4_SPIDEV_BARO */ -- cgit v1.2.3 From b174a6051527dacc858c6ed54b5d113888c5d4de Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 15 Jul 2013 09:11:52 +0400 Subject: multirotor_pos_control: PID ontroller derivative mode fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1d2dd384a..4bae7efa4 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -234,11 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -401,7 +401,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction */ + /* limit maximum setpoint from position offset and preserve direction + * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; -- cgit v1.2.3 From 1b38cf715d85b15f2200d49b64fbe22a05b71937 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 15 Jul 2013 22:15:15 +0200 Subject: Renamed actuator_safety back to actuator_armed, compiling but untested --- src/drivers/ardrone_interface/ardrone_interface.c | 19 +++-- src/drivers/blinkm/blinkm.cpp | 83 ++++++++++++--------- src/drivers/hil/hil.cpp | 18 ++--- src/drivers/mkblctrl/mkblctrl.cpp | 20 ++--- src/drivers/px4fmu/fmu.cpp | 28 +++---- src/drivers/px4io/px4io.cpp | 37 ++++----- .../flow_position_control_main.c | 10 +-- .../flow_position_estimator_main.c | 20 ++--- .../flow_speed_control/flow_speed_control_main.c | 12 +-- src/modules/commander/commander.c | 87 +++++++++++----------- src/modules/commander/commander_helper.h | 2 +- src/modules/commander/state_machine_helper.c | 26 +++---- src/modules/commander/state_machine_helper.h | 4 +- src/modules/gpio_led/gpio_led.c | 14 ++-- src/modules/mavlink/orb_listener.c | 18 ++--- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 10 +-- src/modules/mavlink_onboard/orb_topics.h | 2 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 18 ++--- src/modules/sdlog2/sdlog2.c | 32 ++++---- src/modules/uORB/objects_common.cpp | 9 ++- src/modules/uORB/topics/actuator_armed.h | 58 +++++++++++++++ src/modules/uORB/topics/actuator_safety.h | 66 ---------------- src/modules/uORB/topics/safety.h | 57 ++++++++++++++ src/modules/uORB/topics/vehicle_control_mode.h | 6 +- 26 files changed, 367 insertions(+), 294 deletions(-) create mode 100644 src/modules/uORB/topics/actuator_armed.h delete mode 100644 src/modules/uORB/topics/actuator_safety.h create mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 187820372..b88f61ce8 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -53,9 +53,10 @@ #include #include #include -#include +#include #include -#include +#include +#include #include @@ -244,17 +245,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int led_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_safety_s safety; - safety.armed = false; + struct actuator_armed_s armed; + //XXX is this necessairy? + armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -325,12 +324,12 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* for now only spin if armed and immediately shut down * if in failsafe */ - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index a11f88477..e83a87aa9 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,7 +116,8 @@ #include #include #include -#include +#include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,8 +377,9 @@ BlinkM::led() { static int vehicle_status_sub_fd; + static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; - static int actuator_safety_sub_fd; + static int actuator_armed_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -388,7 +390,8 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; - static int no_data_actuator_safety = 0; + static int no_data_vehicle_control_mode = 0; + static int no_data_actuator_armed = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -401,8 +404,11 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); - actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 1000); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -458,14 +464,16 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; - struct actuator_safety_s actuator_safety; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; - bool new_data_actuator_safety; + bool new_data_vehicle_control_mode; + bool new_data_actuator_armed; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -479,15 +487,26 @@ BlinkM::led() no_data_vehicle_status = 3; } - orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); - if (new_data_actuator_safety) { - orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); - no_data_actuator_safety = 0; + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; } else { - no_data_actuator_safety++; - if(no_data_vehicle_status >= 3) - no_data_vehicle_status = 3; + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); @@ -549,7 +568,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(actuator_safety.armed == false) { + if(actuator_armed.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -573,25 +592,21 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - /* handle 4th led - flightmode indicator */ -#warning fix this -// switch((int)vehicle_status_raw.flight_mode) { -// case VEHICLE_FLIGHT_MODE_MANUAL: -// led_color_4 = LED_AMBER; -// break; -// -// case VEHICLE_FLIGHT_MODE_STAB: -// led_color_4 = LED_YELLOW; -// break; -// -// case VEHICLE_FLIGHT_MODE_HOLD: -// led_color_4 = LED_BLUE; -// break; -// -// case VEHICLE_FLIGHT_MODE_AUTO: -// led_color_4 = LED_GREEN; -// break; -// } + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + + //XXX please check + if (vehicle_control_mode.flag_control_position_enabled) + led_color_4 = LED_GREEN; + else if (vehicle_control_mode.flag_control_velocity_enabled) + led_color_4 = LED_BLUE; + else if (vehicle_control_mode.flag_control_attitude_enabled) + led_color_4 = LED_YELLOW; + else if (vehicle_control_mode.flag_control_manual_enabled) + led_color_4 = LED_AMBER; + else + led_color_4 = LED_OFF; + + } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat�s */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 15fbf8c88..3e30e3292 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,7 +75,7 @@ #include #include -#include +#include #include #include @@ -109,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_safety; + int _t_armed; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -162,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_safety(-1), + _t_armed(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -322,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_safety, 200); /* 5Hz update rate */ + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -335,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_safety; + fds[1].fd = _t_armed; fds[1].events = POLLIN; unsigned num_outputs; @@ -427,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s aa; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); } } ::close(_t_actuators); - ::close(_t_safety); + ::close(_t_armed); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3fc1054e6..06930db38 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,7 +74,7 @@ #include #include #include -#include +#include #include #include @@ -135,7 +135,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; unsigned int _motor; int _px4mode; int _frametype; @@ -248,7 +248,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _t_esc_status(0), @@ -513,8 +513,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -540,7 +540,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; log("starting"); @@ -651,13 +651,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(as.armed && !as.lockdown); + mk_servo_arm(aa.armed && !aa.lockdown); } @@ -700,7 +700,7 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d0499d2fd..41c8c8bb7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,7 @@ #include #include #include -#include +#include #include #include @@ -105,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -175,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -377,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -397,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -500,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = as.armed && !as.lockdown; + bool set_armed = aa.armed && !aa.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -536,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1022,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_safety_s as; + actuator_armed_s aa; - as.armed = true; - as.lockdown = false; + aa.armed = true; + aa.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_safety), &as); + handle = orb_advertise(ORB_ID(actuator_armed), &aa); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 94f231821..ea732eccd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,7 +75,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -178,7 +179,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_actuator_safety; ///< system armed control topic + int _t_actuator_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -360,7 +361,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -505,9 +506,9 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int safety_sub = orb_subscribe(ORB_ID(actuator_armed)); /* fill with initial values, clear updated flag */ - struct actuator_safety_s safety; + struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; @@ -518,7 +519,7 @@ PX4IO::init() if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); break; } @@ -559,7 +560,7 @@ PX4IO::init() orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); } /* wait 50 ms */ @@ -643,8 +644,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -656,7 +657,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -832,21 +833,21 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_safety_s safety; ///< system armed state + actuator_armed_s armed; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (safety.ready_to_arm) { + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1004,10 +1005,10 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct actuator_safety_s safety; + struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(safety), _to_safety, &safety); if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; @@ -1019,9 +1020,9 @@ PX4IO::io_handle_status(uint16_t status) /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); + orb_publish(ORB_ID(safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); + _to_safety = orb_advertise(ORB_ID(safety), &safety); } return ret; diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 0b9404582..621d3253f 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,7 +159,7 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; @@ -171,7 +171,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); @@ -261,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -578,7 +578,7 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 557fdf37c..5e80066a7 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,8 +159,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; @@ -173,8 +173,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + /* subscribe to armed topic */ + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* subscribe to safety topic */ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -270,7 +270,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ @@ -284,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -356,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !safety.armed) + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; @@ -453,7 +453,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 8032a6264..6af955cd7 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -156,7 +156,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -166,7 +166,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -229,8 +229,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the safety topic */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the armed topic */ + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of the control mode */ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ @@ -355,7 +355,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(att_sp_pub); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 2b0b3a415..c4ee605cc 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -76,9 +76,10 @@ #include #include #include -#include +#include #include #include +#include #include #include @@ -177,7 +178,7 @@ int led_off(int led); void do_reboot(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); // int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -253,7 +254,7 @@ void do_reboot() -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -273,13 +274,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -292,14 +293,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -312,7 +313,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -361,7 +362,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -380,7 +381,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -400,7 +401,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -421,7 +422,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -441,7 +442,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -460,7 +461,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -734,12 +735,11 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - /* safety topic */ - struct actuator_safety_s safety; - orb_advert_t safety_pub; - /* Initialize safety with all false */ - memset(&safety, 0, sizeof(safety)); - + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); /* flags for control apps */ struct vehicle_control_mode_s control_mode; @@ -768,8 +768,8 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; @@ -789,10 +789,7 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); - - /* but also subscribe to it */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -844,6 +841,11 @@ int commander_thread_main(int argc, char *argv[]) /* XXX what exactly is this for? */ uint64_t last_print_time = 0; + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -917,7 +919,6 @@ int commander_thread_main(int argc, char *argv[]) /* XXX use this! */ //uint64_t failsave_ll_start_time = 0; - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -936,7 +937,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -990,7 +991,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); + handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } @@ -1003,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1028,7 +1029,7 @@ int commander_thread_main(int argc, char *argv[]) orb_check(safety_sub, &new_data); if (new_data) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ @@ -1170,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1287,7 +1288,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !safety.armed) { + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1579,7 +1580,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); tune_positive(); } @@ -1595,13 +1596,12 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position --> arm */ - if (current_status.flag_control_manual_enabled && - current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + /* check if left stick is in lower right position and we're in manual --> arm */ + if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; tune_positive(); @@ -1704,13 +1704,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !safety.armed) { + if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - } else if (!sp_offboard.armed && safety.armed) { + } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } } else { @@ -1772,7 +1772,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; @@ -1789,7 +1789,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1812,6 +1812,7 @@ int commander_thread_main(int argc, char *argv[]) close(sp_offboard_sub); close(global_position_sub); close(sensor_sub); + close(safety_sub); close(cmd_sub); warnx("exiting"); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index ea96d72a6..b06e85169 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -43,7 +43,7 @@ #include #include -#include +#include #include diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index c15fc91a0..0b241d108 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -57,7 +57,7 @@ #include "commander_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { int ret = ERROR; @@ -73,8 +73,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -86,8 +86,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - safety->armed = false; - safety->ready_to_arm = true; + armed->armed = false; + armed->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -101,7 +101,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -111,7 +111,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -120,8 +120,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -132,8 +132,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; @@ -151,8 +151,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - safety->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_safety), safety_pub, safety); + armed->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_armed), armed_pub, armed); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b553a4b56..e591d2fef 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,11 +46,11 @@ #include #include -#include +#include #include -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 97b585cb7..6eb425705 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -63,8 +63,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; - struct actuator_safety_s safety; - int actuator_safety_sub; + struct actuator_armed_s armed; + int actuator_armed_sub; bool led_state; int counter; }; @@ -233,12 +233,12 @@ void gpio_led_cycle(FAR void *arg) orb_check(priv->vehicle_status_sub, &status_updated); if (status_updated) - orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); /* select pattern for current status */ int pattern = 0; - if (priv->safety.armed) { + if (priv->armed.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -247,10 +247,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->safety.ready_to_arm) { + if (priv->armed.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 95bc8fdc0..57a5d5dd5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_safety_s safety; +struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_0; struct vehicle_attitude_s att; @@ -110,7 +110,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_safety(const struct listener *l); +static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -135,7 +135,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_safety, &mavlink_subs.safety_sub, 0}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && safety.armed) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_safety(const struct listener *l) +l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void @@ -759,8 +759,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 3b968b911..506f73105 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -60,7 +60,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,6 +79,7 @@ struct mavlink_subscriptions { int man_control_sp_sub; int safety_sub; int actuators_sub; + int armed_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 2d5a64ee8..9ed2c6c12 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -273,7 +273,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -284,12 +284,12 @@ get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, co *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (safety->hil_enabled) { + if (control_mode->flag_system_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (safety->armed) { + if (armed->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -370,7 +370,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* XXX this is never written? */ struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; - struct actuator_safety_s safety; + struct actuator_armed_s armed; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -438,7 +438,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 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(&control_mode, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f01f09e12..1b49c9ce4 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index c6a2e52bf..c84b6fd26 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -51,5 +51,5 @@ 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_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 70a61fc4e..20502c0ea 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -94,8 +94,8 @@ mc_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -121,7 +121,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -210,10 +210,10 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(safety_sub, &updated); + orb_check(armed_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* get a local copy of manual setpoint */ @@ -261,7 +261,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || control_mode.flag_control_manual_enabled != flag_control_manual_enabled || - safety.armed != flag_armed) { + armed.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -275,7 +275,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + if (!flag_control_attitude_enabled && armed.armed) { att_sp.yaw_body = att.yaw; } @@ -366,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = control_mode.flag_control_manual_enabled; flag_control_position_enabled = control_mode.flag_control_position_enabled; flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; - flag_armed = safety.armed; + flag_armed = armed.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 80ee3adee..b20d38b5e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,7 +60,7 @@ #include #include -#include +#include #include #include #include @@ -194,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct actuator_safety_s *safety); +static void handle_status(struct actuator_armed_s *armed); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -628,8 +628,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); - struct actuator_safety_s buf_safety; - memset(&buf_safety, 0, sizeof(buf_safety)); + struct actuator_armed_s buf_armed; + memset(&buf_armed, 0, sizeof(buf_armed)); /* warning! using union here to save memory, elements should be used separately! */ union { @@ -659,7 +659,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int safety_sub; + int armed_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -717,9 +717,9 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY --- */ - subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - fds[fdsc_count].fd = subs.safety_sub; + /* --- ACTUATOR ARMED --- */ + subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + fds[fdsc_count].fd = subs.armed_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -896,12 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- SAFETY- LOG MANAGEMENT --- */ + /* --- ARMED- LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); + orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed); if (log_when_armed) { - handle_status(&buf_safety); + handle_status(&buf_armed); } handled_topics++; @@ -912,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); //if (log_when_armed) { - // handle_status(&buf_safety); + // handle_status(&buf_armed); //} //handled_topics++; @@ -944,7 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1348,10 +1348,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct actuator_safety_s *safety) +void handle_status(struct actuator_armed_s *armed) { - if (safety->armed != flag_system_armed) { - flag_system_armed = safety->armed; + if (armed->armed != flag_system_armed) { + flag_system_armed = armed->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a061fe676..ed77bb7ef 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); + #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); @@ -153,8 +156,8 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -#include "topics/actuator_safety.h" -ORB_DEFINE(actuator_safety, struct actuator_safety_s); +#include "topics/actuator_armed.h" +ORB_DEFINE(actuator_armed, struct actuator_armed_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h new file mode 100644 index 000000000..6e944ffee --- /dev/null +++ b/src/modules/uORB/topics/actuator_armed.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 actuator_armed.h + * + * Actuator armed topic + * + */ + +#ifndef TOPIC_ACTUATOR_ARMED_H +#define TOPIC_ACTUATOR_ARMED_H + +#include +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_armed_s { + + uint64_t timestamp; + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_armed); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h deleted file mode 100644 index c431217ab..000000000 --- a/src/modules/uORB/topics/actuator_safety.h +++ /dev/null @@ -1,66 +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 actuator_controls.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller - */ - -#ifndef TOPIC_ACTUATOR_SAFETY_H -#define TOPIC_ACTUATOR_SAFETY_H - -#include -#include "../uORB.h" - -/** global 'actuator output is live' control. */ -struct actuator_safety_s { - - uint64_t timestamp; - bool safety_switch_available; /**< Set to true if a safety switch is connected */ - bool safety_off; /**< Set to true if safety is off */ - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ -}; - -ORB_DECLARE(actuator_safety); - -#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 000000000..a5d21cd4a --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 safety.h + * + * Safety topic to pass safety state from px4io driver to commander + * This concerns only the safety button of the px4io but has nothing to do + * with arming/disarming. + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +struct safety_s { + + uint64_t timestamp; + bool safety_switch_available; /**< Set to true if a safety switch is connected */ + bool safety_off; /**< Set to true if safety is off */ +}; + +ORB_DECLARE(safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 177e30898..02bf5568a 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -69,9 +69,13 @@ struct vehicle_control_mode_s bool flag_system_emergency; bool flag_preflight_calibration; + // XXX needs yet to be set by state machine helper + bool flag_system_hil_enabled; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; + // XXX shouldn't be necessairy + //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ -- cgit v1.2.3 From ce8e2fd72668d64e3a56d3b1df5d7f9079fcdf55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 09:00:21 +0200 Subject: Fixed compile error due to bad merge --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e9f26b0d3..8f8555f1f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1510,7 +1510,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) - bits &= ~PX4IO_RELAY1; + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; } @@ -1518,7 +1518,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_SET: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); @@ -1527,7 +1527,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); -- cgit v1.2.3 From 2f76c811474aa34dd11973df491283278234957a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 09:08:05 +0200 Subject: More compile fixes --- .gitignore | 8 +++++++- makefiles/config_px4fmu-v2_default.mk | 3 ++- src/modules/gpio_led/gpio_led.c | 10 +++++----- src/modules/px4iofirmware/dsm.c | 5 +++++ 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/.gitignore b/.gitignore index 0372b60c8..5a4f7683c 100644 --- a/.gitignore +++ b/.gitignore @@ -24,4 +24,10 @@ Firmware.sublime-workspace Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ -NuttX \ No newline at end of file +NuttX +nuttx-configs/px4io-v2/src/.depend +nuttx-configs/px4io-v2/src/Make.dep +nuttx-configs/px4io-v2/src/libboard.a +nuttx-configs/px4io-v1/src/.depend +nuttx-configs/px4io-v1/src/Make.dep +nuttx-configs/px4io-v1/src/libboard.a \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index c4499048c..8e64bd754 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -26,7 +26,8 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott_telemetry +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += modules/sensors diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1aef739c7..7466dfdb9 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -109,19 +109,19 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[3], "a1")) { use_io = true; - pin = PX4IO_ACC1; + pin = PX4IO_P_SETUP_RELAYS_ACC1; } else if (!strcmp(argv[3], "a2")) { use_io = true; - pin = PX4IO_ACC2; + pin = PX4IO_P_SETUP_RELAYS_ACC2; } else if (!strcmp(argv[3], "r1")) { use_io = true; - pin = PX4IO_RELAY1; + pin = PX4IO_P_SETUP_RELAYS_POWER1; } else if (!strcmp(argv[3], "r2")) { use_io = true; - pin = PX4IO_RELAY2; + pin = PX4IO_P_SETUP_RELAYS_POWER2; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -142,7 +142,7 @@ int gpio_led_main(int argc, char *argv[]) char pin_name[24]; if (use_io) { - if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); } else { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ab6e3fec4..598bcee34 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -110,6 +110,10 @@ dsm_bind(uint16_t cmd, int pulses) if (dsm_fd < 0) return; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // XXX implement + #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 +#else switch (cmd) { case dsm_bind_power_down: // power down DSM satellite @@ -135,6 +139,7 @@ dsm_bind(uint16_t cmd, int pulses) stm32_configgpio(GPIO_USART1_RX); break; } +#endif } bool -- cgit v1.2.3 From 3e161049ac4e953f8c0084b1872b544de6189f5d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 09:24:21 +0200 Subject: Got rid of useless orb_receive_loop, moved some helper functions --- src/modules/commander/commander.c | 196 ++++++++----------------------- src/modules/commander/commander_helper.c | 46 ++++++++ src/modules/commander/commander_helper.h | 7 +- 3 files changed, 103 insertions(+), 146 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c4ee605cc..a74c9ebe9 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -132,8 +132,7 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ -/* File descriptors */ -static int leds; +/* Mavlink file descriptors */ static int mavlink_fd; /* flags */ @@ -159,8 +158,9 @@ enum { } low_prio_task; -/* pthread loops */ -void *orb_receive_loop(void *arg); +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -170,88 +170,16 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int led_init(void); -void led_deinit(void); -int led_toggle(int led); -int led_on(int led); -int led_off(int led); -void do_reboot(void); - - +/** + * React to commands that are sent e.g. from the mavlink module. + */ void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); -// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - /** * Print the correct usage. */ void usage(const char *reason); -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - - -int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -void led_deinit() -{ - close(leds); -} - -int led_toggle(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); -} - -int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -void do_reboot() -{ - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -} - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) @@ -317,8 +245,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - do_reboot(); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { /* system may return here */ @@ -526,59 +456,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - /* * Provides a coarse estimate of remaining battery power. * @@ -709,9 +586,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("[commander] starting"); - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; /* initialize */ @@ -807,12 +682,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2048); @@ -905,6 +774,11 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1073,6 +947,39 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update subsystem */ + orb_check(subsys_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + current_status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + current_status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + current_status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_health &= ~info.subsystem_type; + } + } + /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { @@ -1801,8 +1708,6 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); pthread_join(commander_low_prio_thread, NULL); /* close fds */ @@ -1814,6 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(safety_sub); close(cmd_sub); + close(subsys_sub); warnx("exiting"); fflush(stdout); diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index a75b5dec3..fb5c47885 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -51,6 +51,7 @@ #include #include #include +#include #include "commander_helper.h" @@ -127,3 +128,48 @@ void tune_stop() ioctl(buzzer, TONE_SET_ALARM, 0); } +static int leds; + +int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +void led_deinit() +{ + close(leds); +} + +int led_toggle(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); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} \ No newline at end of file diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index b06e85169..71a257c86 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -60,7 +60,12 @@ void tune_negative(void); int tune_arm(void); int tune_critical_bat(void); int tune_low_bat(void); - void tune_stop(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); + #endif /* COMMANDER_HELPER_H_ */ -- cgit v1.2.3 From 08926019ea4203760a225e957d27328862182ce1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 09:35:31 +0200 Subject: Just some reordering in commander --- src/modules/commander/commander.c | 183 +++++++++++-------------------- src/modules/commander/commander_helper.c | 43 ++++++++ src/modules/commander/commander_helper.h | 9 ++ 3 files changed, 118 insertions(+), 117 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index a74c9ebe9..d7bf6ac57 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -159,16 +159,19 @@ enum { /** - * Loop that runs at a lower rate and priority for calibration and parameter tasks. + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). */ -void *commander_low_prio_loop(void *arg); - __EXPORT int commander_main(int argc, char *argv[]); /** - * Mainloop of commander. + * Print the correct usage. */ -int commander_thread_main(int argc, char *argv[]); +void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. @@ -176,11 +179,67 @@ int commander_thread_main(int argc, char *argv[]); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** - * Print the correct usage. + * Mainloop of commander. */ -void usage(const char *reason); +int commander_thread_main(int argc, char *argv[]); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); + + +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { @@ -456,116 +515,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index fb5c47885..199f73e6c 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -172,4 +173,46 @@ int led_on(int led) int led_off(int led) { return ioctl(leds, LED_OFF, led); +} + + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; } \ No newline at end of file diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 71a257c86..c621b9823 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -68,4 +68,13 @@ int led_toggle(int led); int led_on(int led); int led_off(int led); +/** + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + #endif /* COMMANDER_HELPER_H_ */ -- cgit v1.2.3 From 6dc3fcd1ad108bc830231c1da50982e18eb7f799 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 10:05:51 +0200 Subject: Some more commander cleanup, param update handling code was doubled --- src/modules/commander/commander.c | 79 ++++++++++++--------------------------- 1 file changed, 24 insertions(+), 55 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index d7bf6ac57..b457d98a1 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -39,11 +39,6 @@ * @file commander.c * Main system state machine implementation. * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * */ #include "commander.h" @@ -656,9 +651,23 @@ int commander_thread_main(int argc, char *argv[]) uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; - /* XXX what exactly is this for? */ + /* To remember when last notification was sent */ uint64_t last_print_time = 0; + float voltage_previous = 0.0f; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + bool new_data = false; + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; @@ -702,6 +711,7 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + /* Subscribe to differential pressure topic */ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); @@ -717,7 +727,7 @@ int commander_thread_main(int argc, char *argv[]) struct parameter_update_s param_changed; memset(¶m_changed, 0, sizeof(param_changed)); - /* subscribe to battery topic */ + /* Subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); @@ -728,28 +738,14 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - /* now initialized */ commander_initialized = true; thread_running = true; - uint64_t start_time = hrt_absolute_time(); - - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; + start_time = hrt_absolute_time(); while (!thread_should_exit) { - /* Get current values */ - bool new_data; - /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -758,11 +754,10 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); + warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ @@ -816,37 +811,6 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } - - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - /* update parameters */ - if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - } - } /* update safety topic */ orb_check(safety_sub, &new_data); @@ -1664,11 +1628,16 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); + close(local_position_sub); close(global_position_sub); + close(gps_sub); close(sensor_sub); close(safety_sub); close(cmd_sub); close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); warnx("exiting"); fflush(stdout); -- cgit v1.2.3 From 6902177b997906dc0003adc9e8210d1901f3dafc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 12:45:43 +0200 Subject: Default to 2000 dps for L3GD20 --- src/drivers/l3gd20/l3gd20.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f47481823..28d6397c9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -334,10 +334,10 @@ L3GD20::init() write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ - set_range(500); /* default to 500dps */ + set_range(2000); /* default to 2000dps */ set_samplerate(0); /* max sample rate */ ret = OK; -- cgit v1.2.3 From c8aca814ca4bff633dfd4a8341c08a2d4b8074fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 12:46:23 +0200 Subject: Improved comments --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 28d6397c9..a9af6711b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -351,7 +351,7 @@ L3GD20::probe() /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); - /* verify that the device is attached and functioning */ + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) return OK; -- cgit v1.2.3 From 76edfa896b57782d7a4333bcf7a582952cb0fae4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 13:24:02 +0200 Subject: Fixed disarming bug, use flag instead of mode switch --- src/modules/commander/commander.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index b457d98a1..144fda79a 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1387,8 +1387,7 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - // printf("checking\n"); - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1396,7 +1395,7 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) { - if (current_status.mode_switch != MODE_SWITCH_MANUAL) { + if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { @@ -1417,8 +1416,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in manual --> arm */ - if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && - sp_man.yaw > STICK_ON_OFF_LIMIT && + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); -- cgit v1.2.3 From 39d88471896ac46c4aae475f7d6c73e44e7b5f25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 16:43:11 +0200 Subject: sercon is only used by APM now --- ROMFS/px4fmu_common/init.d/rcS | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb..498c93f28 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -42,30 +42,31 @@ then sh /fs/microsd/etc/rc.txt fi -# -# Check for USB host -# -if [ $USB != autoconnect ] +# if this is an APM build then there will be a rc.APM script +# from an EXTERNAL_SCRIPTS build option +if [ -f /etc/init.d/rc.APM ] then - echo "[init] not connecting USB" -else - if sercon + + # + # Check for USB host + # + if [ $USB != autoconnect ] then - echo "[init] USB interface connected" + echo "[init] not connecting USB" else - if [ -f /dev/ttyACM0 ] - echo "[init] NSH via USB" + if sercon then + echo "[init] USB interface connected" else - echo "[init] No USB connected" + if [ -f /dev/ttyACM0 ] + echo "[init] NSH via USB" + then + else + echo "[init] No USB connected" + fi fi fi -fi -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then echo Running rc.APM # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM -- cgit v1.2.3 From bcdedd9a35a5b9ebf3851a0d472adab8d3e7edac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 18:55:32 +0200 Subject: Changed location of lots of flags and conditions, needs testing and more work --- src/drivers/blinkm/blinkm.cpp | 2 +- src/drivers/px4io/px4io.cpp | 18 +-- .../attitude_estimator_ekf_main.cpp | 14 +-- .../attitude_estimator_so3_comp_main.cpp | 14 +-- src/modules/commander/commander.c | 122 ++++++++++----------- src/modules/commander/state_machine_helper.c | 11 +- src/modules/commander/state_machine_helper.h | 2 +- src/modules/controllib/fixedwing.cpp | 24 ++-- .../fixedwing_backside/fixedwing_backside_main.cpp | 6 +- src/modules/mavlink/mavlink.c | 15 ++- src/modules/mavlink/orb_listener.c | 5 +- src/modules/mavlink_onboard/mavlink.c | 4 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 32 +++--- src/modules/uORB/topics/vehicle_control_mode.h | 3 - src/modules/uORB/topics/vehicle_status.h | 37 ++----- 16 files changed, 149 insertions(+), 164 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index e83a87aa9..9bb020a6b 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -536,7 +536,7 @@ BlinkM::led() /* looking for lipo cells that are connected */ printf(" checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf(" cells found:%d\n", num_of_cells); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea732eccd..827b0bb00 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -77,7 +77,7 @@ #include #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic int _t_actuator_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status + int _t_vehicle_control_mode; ///< vehicle control mode topic int _t_param; ///< parameter update topic /* advertised topics */ @@ -362,7 +362,7 @@ PX4IO::PX4IO() : _alarms(0), _t_actuators(-1), _t_actuator_armed(-1), - _t_vstatus(-1), + _t_vehicle_control_mode(-1), _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), @@ -647,8 +647,8 @@ PX4IO::task_main() _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ @@ -659,7 +659,7 @@ PX4IO::task_main() fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; - fds[2].fd = _t_vstatus; + fds[2].fd = _t_vehicle_control_mode; fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; @@ -834,10 +834,10 @@ int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state - vehicle_status_s vstatus; ///< overall system state + vehicle_control_mode_s control_mode; ///< vehicle_control_mode orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; @@ -853,7 +853,7 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } - if (vstatus.flag_external_manual_override_ok) { + if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d8b40ac3b..d1b941ffe 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include @@ -214,8 +214,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -228,8 +228,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode*/ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -281,9 +281,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39..2a06f1628 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include @@ -545,8 +545,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -559,8 +559,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -610,9 +610,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 144fda79a..c4dc495f6 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -171,7 +171,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -236,7 +236,7 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -248,7 +248,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -256,13 +256,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -275,14 +275,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -295,7 +295,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -346,7 +346,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -365,7 +365,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -426,7 +426,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -445,7 +445,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -580,7 +580,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = true; /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ // current_status.flag_vector_flight_mode_ok = false; @@ -637,32 +637,24 @@ int commander_thread_main(int argc, char *argv[]) pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); /* Start monitoring loop */ - uint16_t counter = 0; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; /* To remember when last notification was sent */ uint64_t last_print_time = 0; float voltage_previous = 0.0f; + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + uint64_t last_idle_time = 0; uint64_t start_time = 0; - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - bool state_changed = true; bool param_init_forced = true; @@ -764,10 +756,10 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || current_status.system_type == VEHICLE_TYPE_HEXAROTOR || current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; + control_mode.flag_external_manual_override_ok = false; } else { - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; } /* check and update system / component ID */ @@ -809,7 +801,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -848,16 +840,20 @@ int commander_thread_main(int argc, char *argv[]) orb_check(battery_sub, &new_data); if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; } /* update subsystem */ @@ -897,14 +893,15 @@ int commander_thread_main(int argc, char *argv[]) if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* XXX if armed */ - if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { + if (armed.armed) { /* armed, solid */ led_on(LED_AMBER); - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); } @@ -926,15 +923,16 @@ int commander_thread_main(int argc, char *argv[]) led_off(LED_BLUE); } - /* toggle GPS led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_BLUE); - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle arming (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - } + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } } @@ -948,35 +946,29 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); } low_voltage_counter++; } /* Critical, this is rather an emergency, change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - // XXX implement this - // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); + tune_critical_bat(); + // XXX implement state change here } critical_voltage_counter++; @@ -1585,7 +1577,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; + voltage_previous = current_status.battery_voltage; /* play tone according to evaluation result */ @@ -1595,10 +1587,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = true; /* Trigger audio event for low battery */ - } else if (bat_remain < 0.1f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (bat_remain < 0.2f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0b241d108..792ead8f3 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -510,7 +510,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /** * Transition from one hil state to another */ -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -530,7 +530,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = false; + current_control_mode->flag_system_hil_enabled = false; mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); valid_transition = true; } @@ -541,7 +541,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = true; + current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -558,8 +558,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status current_status->counter++; current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e591d2fef..75fdd224c 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -54,6 +54,6 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 9435959e9..0cfcfd51d 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" +#if 0 namespace control { @@ -277,11 +278,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { @@ -343,11 +345,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } #warning fix this // } @@ -382,3 +385,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() } // namespace control +#endif \ No newline at end of file diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526e..677a86771 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -150,12 +150,14 @@ int control_demo_thread_main(int argc, char *argv[]) using namespace control; - fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); +#warning fix this +// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); thread_running = true; while (!thread_should_exit) { - autopilot.update(); +#warning fix this +// autopilot.update(); } warnx("exiting."); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a2758a45c..5f683c443 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -191,7 +191,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* HIL */ - if (v_status.flag_hil_enabled) { + if (v_status.hil_state == HIL_STATE_ON) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -234,11 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_calibration) { + if (v_status.preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; @@ -677,7 +677,10 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, @@ -685,8 +688,8 @@ int mavlink_thread_main(int argc, char *argv[]) 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_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 57a5d5dd5..d088d421e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -272,7 +272,10 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 9ed2c6c12..c5dbd00dd 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -449,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[]) 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_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b20d38b5e..ab3983019 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -945,8 +945,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ - log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; - log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; + log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; LOGBUFFER_WRITE_AND_COUNT(STAT); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index acc0c2717..5e334638f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -165,7 +165,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -352,9 +352,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -411,7 +411,7 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), @@ -941,21 +941,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !_hil_enabled) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1348,12 +1348,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* * do advertisements @@ -1406,7 +1406,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 02bf5568a..8481a624f 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -66,9 +66,6 @@ struct vehicle_control_mode_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_system_emergency; - bool flag_preflight_calibration; - // XXX needs yet to be set by state machine helper bool flag_system_hil_enabled; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2bcd57f4b..ec08a6c13 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -177,14 +177,11 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - /* system flags - these represent the state predicates */ - mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; - - bool condition_system_emergency; + bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; bool condition_system_returned_to_home; @@ -195,28 +192,6 @@ struct vehicle_status_s bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - // bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - //bool flag_armed; /**< true is motors / actuators are armed */ - //bool flag_safety_off; /**< true if safety is off */ - bool flag_system_emergency; - bool flag_preflight_calibration; - - // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // bool flag_auto_enabled; - // bool flag_control_rates_enabled; /**< true if rates are stabilized */ - // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - // bool flag_control_position_enabled; /**< true if position is controlled */ - - // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - // bool flag_preflight_accel_calibration; - // bool flag_preflight_airspeed_calibration; - bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ @@ -230,14 +205,20 @@ struct vehicle_status_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; + bool preflight_calibration; + + bool system_emergency; + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; + float load; - float voltage_battery; - float current_battery; + float battery_voltage; + float battery_current; float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ uint16_t drop_rate_comm; uint16_t errors_comm; -- cgit v1.2.3 From 35a519a2ea36f6ae69a2084428cdff6ed91e7b07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Jul 2013 20:36:50 +0200 Subject: Fixed sensor driver name --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 62c7184b8..be481aa8d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -25,7 +25,7 @@ then else echo "using L3GD20 and LSM303D" l3gd20 start - lsm303 start + lsm303d start fi # -- cgit v1.2.3 From e86e2a093861e51fde836f8cd383b09536ca0542 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 07:57:02 +0200 Subject: Add additional file name options --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8f8555f1f..1e00f9668 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1890,7 +1890,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[5]; /* work out what we're uploading... */ if (argc > 2) { @@ -1900,7 +1900,9 @@ px4io_main(int argc, char *argv[]) } else { fn[0] = "/fs/microsd/px4io.bin"; fn[1] = "/etc/px4io.bin"; - fn[2] = nullptr; + fn[2] = "/fs/microsd/px4io2.bin"; + fn[3] = "/etc/px4io2.bin"; + fn[4] = nullptr; } up = new PX4IO_Uploader; -- cgit v1.2.3 From e266f6d425eb606e35509181331d0286713a4a34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 08:37:02 +0200 Subject: WIP on microSD support --- nuttx-configs/px4fmu-v2/nsh/defconfig | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index efbb883a5..b068b1de2 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -302,7 +302,23 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # # SDIO Configuration # -CONFIG_SDIO_PRI=128 + +# +# Maintain legacy build behavior (revisit) +# + +CONFIG_MMCSD=y +#CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y + +# +# STM32 SDIO-based MMC/SD driver +# +CONFIG_SDIO_DMA=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_MMCSUPPORT=n +CONFIG_MMCSD_HAVECARDDETECT=n # # USB Host Configuration @@ -361,6 +377,7 @@ CONFIG_ARCH_BOARD="px4fmu-v2" # # Common Board Options # +CONFIG_NSH_MMCSDSLOTNO=0 CONFIG_NSH_MMCSDMINOR=0 # -- cgit v1.2.3 From 72f58829c31c831d98106ceb443c97925fcb9de5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 08:49:15 +0200 Subject: Fixed microSD, operational fine, will need more work with CCM SRAM. --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index b068b1de2..839d476da 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -253,7 +253,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_CCMEXCLUDE=y CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set -- cgit v1.2.3 From c4248c055f098e775e57a7f0ea6ffda49e930b01 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 19:54:47 +0400 Subject: position_estimator_inav: minor fixes --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3b0fbd782..a9bc09e0d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -115,6 +115,7 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(0); } + verbose_mode = false; if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -379,10 +380,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_NED[2] += CONSTANTS_ONE_G; accel_corr[0] = accel_NED[0] - x_est[2]; accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] - z_est[2]; + accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { memset(accel_corr, 0, sizeof(accel_corr)); -- cgit v1.2.3 From 5679a1b2b17085ab4aafee6b97790c340785a6a6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 17 Jul 2013 09:19:17 -0700 Subject: Fix handling of register read operation errors. --- src/drivers/px4io/px4io.cpp | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1e00f9668..b019c4fc5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -256,7 +256,7 @@ private: * @param offset Register offset to start writing at. * @param values Pointer to array of values to write. * @param num_values The number of values to write. - * @return Zero if all values were successfully written. + * @return OK if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -266,7 +266,7 @@ private: * @param page Register page to write to. * @param offset Register offset to write to. * @param value Value to write. - * @return Zero if the value was written successfully. + * @return OK if the value was written successfully. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); @@ -277,7 +277,7 @@ private: * @param offset Register offset to start reading from. * @param values Pointer to array where values should be stored. * @param num_values The number of values to read. - * @return Zero if all values were successfully read. + * @return OK if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); @@ -1149,9 +1149,11 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != num_values) + if (ret != num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); - return ret; + return -1; + } + return OK; } int @@ -1169,10 +1171,12 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } - int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); - if (ret != num_values) + int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + if (ret != num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); - return ret; + return -1; + } + return OK; } uint32_t @@ -1180,7 +1184,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1)) + if (io_reg_get(page, offset, &value, 1) != OK) return _io_reg_get_error; return value; @@ -1193,7 +1197,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); - if (ret) + if (ret != OK) return ret; value &= ~clearbits; value |= setbits; @@ -1500,9 +1504,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - - *(uint32_t *)arg = value; + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; break; } -- cgit v1.2.3 From 17366c4b0d2bbcb3d0705bac1a7e4bc737e0bf40 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 22:22:51 +0400 Subject: multirotor_pos_control: fixes for AUTO mode, minor cleanup --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 4bae7efa4..e5a3f3e54 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -333,21 +333,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; } - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); /* publish local position setpoint as projection of global position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } } - if (reset_sp_alt) { + if (status.flag_control_manual_enabled && reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + if (status.flag_control_manual_enabled && status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; -- cgit v1.2.3 From 68b7e0315568efc91a067e161d134b00c1c7e132 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 22:27:22 +0400 Subject: sdlog2: copyright fix --- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6641d88f3..1d34daf58 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -2,7 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier - * Anton Babushkin + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,7 +40,7 @@ * does the heavy SD I/O in a low-priority worker thread. * * @author Lorenz Meier - * @author Anton Babushkin + * @author Anton Babushkin */ #include diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 06e640b5b..95d19d955 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * Log messages and structures definition. * - * @author Anton Babushkin + * @author Anton Babushkin */ #ifndef SDLOG2_MESSAGES_H_ -- cgit v1.2.3 From 17445b0fbb57708816a535b87611f439d6e63c31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jul 2013 22:54:05 +0200 Subject: Added led support to FMUv2 --- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/boards/px4fmu/px4fmu_init.c | 2 +- src/drivers/boards/px4fmuv2/module.mk | 3 +- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 85 +++++++++++++++++++++++++++++++ src/drivers/boards/px4fmuv2/px4fmu_init.c | 5 +- 5 files changed, 91 insertions(+), 5 deletions(-) create mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_led.c diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8e64bd754..73dc8bd9d 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -14,6 +14,7 @@ MODULES += drivers/device MODULES += drivers/stm32 MODULES += drivers/stm32/adc MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led MODULES += drivers/px4fmu MODULES += drivers/px4io MODULES += drivers/boards/px4fmuv2 diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 212a92cfa..36af2298c 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void) /* initial LED state */ drv_led_start(); led_off(LED_AMBER); - led_on(LED_BLUE); + led_off(LED_BLUE); /* Configure SPI-based devices */ diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk index eb8841e4d..b405165e1 100644 --- a/src/drivers/boards/px4fmuv2/module.mk +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -6,4 +6,5 @@ SRCS = px4fmu_can.c \ px4fmu_init.c \ px4fmu_pwm_servo.c \ px4fmu_spi.c \ - px4fmu_usb.c + px4fmu_usb.c \ + px4fmu2_led.c diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c new file mode 100644 index 000000000..bbf29521b --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "px4fmu_internal.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 0) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 0) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c index 57f2812b8..c49111632 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_init.c @@ -192,9 +192,8 @@ __EXPORT int nsh_archinitialize(void) (hrt_callout)stm32_serial_dma_poll, NULL); - // initial LED state - // XXX need to make this work again -// drv_led_start(); + /* initial LED state */ + drv_led_start(); up_ledoff(LED_AMBER); /* Configure SPI-based devices */ -- cgit v1.2.3 From a19e0f2f9c5678b3e1ae56fd48ec7c95eed36ba7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 09:45:27 +0200 Subject: Small fix for make distclean, Linux find doesn't seem to know the -depth n argument --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 9905f8a63..a703bef4c 100644 --- a/Makefile +++ b/Makefile @@ -174,7 +174,7 @@ clean: distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && find . -type l -depth 1 -delete) + $(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete) # # Print some help text -- cgit v1.2.3 From 349c9624694ff0d17d10523470ff62b34356207e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 10:01:43 +0200 Subject: Compiling / executing WIP on leds, leds not operational yet --- nuttx-configs/px4fmu-v2/nsh/defconfig | 47 +++++++++++++++++++++++-------- src/drivers/boards/px4fmuv2/module.mk | 2 +- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 2 +- src/drivers/led/led.cpp | 4 +-- 4 files changed, 39 insertions(+), 16 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 839d476da..70500e4b0 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -242,6 +242,9 @@ CONFIG_STM32_ADC=y CONFIG_STM32_SPI=y CONFIG_STM32_I2C=y +CONFIG_ARCH_HAVE_LEDS=y +# CONFIG_ARCH_LEDS is not set + # # Alternate Pin Mapping # @@ -274,9 +277,9 @@ CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set -CONFIG_USART3_RXDMA=y +# CONFIG_USART3_RXDMA is not set # CONFIG_UART4_RS485 is not set -CONFIG_UART4_RXDMA=y +# CONFIG_UART4_RXDMA is not set # CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set # CONFIG_USART6_RXDMA is not set @@ -284,6 +287,26 @@ CONFIG_UART4_RXDMA=y CONFIG_USART8_RXDMA=y CONFIG_STM32_USART_SINGLEWIRE=y + +# Previous: +## CONFIG_USART1_RS485 is not set +#CONFIG_USART1_RXDMA=y +## CONFIG_USART2_RS485 is not set +#CONFIG_USART2_RXDMA=y +## CONFIG_USART3_RS485 is not set +#CONFIG_USART3_RXDMA=y +## CONFIG_UART4_RS485 is not set +#CONFIG_UART4_RXDMA=y +## CONFIG_UART5_RXDMA is not set +## CONFIG_USART6_RS485 is not set +## CONFIG_USART6_RXDMA is not set +## CONFIG_USART7_RXDMA is not set +#CONFIG_USART8_RXDMA=y +#CONFIG_STM32_USART_SINGLEWIRE=y + + + + # # SPI Configuration # @@ -525,8 +548,8 @@ CONFIG_NO_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=16 -CONFIG_USART1_TXBUFSIZE=16 +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -551,7 +574,7 @@ CONFIG_USART2_OFLOWCONTROL=y # CONFIG_USART3_RXBUFSIZE=512 CONFIG_USART3_TXBUFSIZE=512 -CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 CONFIG_USART3_2STOP=0 @@ -573,9 +596,9 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=16 -CONFIG_USART6_TXBUFSIZE=16 -CONFIG_USART6_BAUD=115200 +CONFIG_USART6_RXBUFSIZE=256 +CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 CONFIG_USART6_2STOP=0 @@ -585,8 +608,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=128 -CONFIG_UART7_TXBUFSIZE=128 +CONFIG_UART7_RXBUFSIZE=256 +CONFIG_UART7_TXBUFSIZE=256 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -597,8 +620,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=128 -CONFIG_UART8_TXBUFSIZE=128 +CONFIG_UART8_RXBUFSIZE=256 +CONFIG_UART8_TXBUFSIZE=256 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk index b405165e1..99d37eeca 100644 --- a/src/drivers/boards/px4fmuv2/module.mk +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -3,7 +3,7 @@ # SRCS = px4fmu_can.c \ - px4fmu_init.c \ + px4fmu2_init.c \ px4fmu_pwm_servo.c \ px4fmu_spi.c \ px4fmu_usb.c \ diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c index bbf29521b..85177df56 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 04b565358..27e43b245 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -110,7 +110,7 @@ LED *gLED; } void -drv_led_start() +drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; -- cgit v1.2.3 From d84824dd4f1d076894df82034868c4655cf7c530 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jul 2013 10:35:36 +0200 Subject: Compile fixes --- src/drivers/boards/px4fmuv2/px4fmu2_init.c | 262 +++++++++++++++++++++++++++++ src/drivers/boards/px4fmuv2/px4fmu_init.c | 262 ----------------------------- 2 files changed, 262 insertions(+), 262 deletions(-) create mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_init.c delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_init.c diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c new file mode 100644 index 000000000..535e075b2 --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu2_init.c @@ -0,0 +1,262 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "px4fmu_internal.h" +#include + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_SERVO_VALID); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + //drv_led_start(); + up_ledoff(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\n"); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 375000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + + message("[boot] Successfully initialized SPI port 2\n"); + + #ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { + message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { + message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + + message("[boot] Initialized SDIO\n"); + #endif + + return OK; +} diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu_init.c deleted file mode 100644 index c49111632..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ /dev/null @@ -1,262 +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 px4fmu_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include "px4fmu_internal.h" -#include - -#include - -#include -#include - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - /* configure SPI interfaces */ - stm32_spiinitialize(); - - /* configure LEDs */ - up_ledinit(); -} - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -static struct spi_dev_s *spi1; -static struct spi_dev_s *spi2; -static struct sdio_dev_s *sdio; - -#include - -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif - -__EXPORT int nsh_archinitialize(void) -{ - - /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ - stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ - stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ - stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ - stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ - - /* configure power supply control/sense pins */ - stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_configgpio(GPIO_VDD_BRICK_VALID); - stm32_configgpio(GPIO_VDD_SERVO_VALID); - stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); - stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); - - /* configure the high-resolution time/callout interface */ - hrt_init(); - - /* configure CPU load estimation */ -#ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); -#endif - - /* set up the serial DMA polling */ - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - - /* initial LED state */ - drv_led_start(); - up_ledoff(LED_AMBER); - - /* Configure SPI-based devices */ - - spi1 = up_spiinitialize(1); - - if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* Default SPI1 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); - SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port 1\n"); - - /* Get the SPI port for the FRAM */ - - spi2 = up_spiinitialize(2); - - if (!spi2) { - message("[boot] FAILED to initialize SPI port 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 375000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, SPIDEV_FLASH, false); - - message("[boot] Successfully initialized SPI port 2\n"); - - #ifdef CONFIG_MMCSD - /* First, get an instance of the SDIO interface */ - - sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); - if (!sdio) { - message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; - } - - /* Now bind the SDIO interface to the MMC/SD driver */ - int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); - if (ret != OK) { - message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; - } - - /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ - sdio_mediachange(sdio, true); - - message("[boot] Initialized SDIO\n"); - #endif - - return OK; -} -- cgit v1.2.3 From 3bea32af8d41585976b78c6dad2701df4180659f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 13:23:14 +0200 Subject: Add HRT and PPM again in IO defconfig --- nuttx-configs/px4io-v2/nsh/defconfig | 22 ++++++++++++++++++++++ nuttx-configs/px4io-v2/test/defconfig | 22 ++++++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 846ea8fb9..4111e70eb 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -195,6 +195,28 @@ SERIAL_HAVE_CONSOLE_DMA=y CONFIG_USART2_RXDMA=n CONFIG_USART3_RXDMA=y +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + # # General build options # diff --git a/nuttx-configs/px4io-v2/test/defconfig b/nuttx-configs/px4io-v2/test/defconfig index 19dfefdf8..132c40d80 100755 --- a/nuttx-configs/px4io-v2/test/defconfig +++ b/nuttx-configs/px4io-v2/test/defconfig @@ -187,6 +187,28 @@ CONFIG_USART1_2STOP=0 CONFIG_USART2_2STOP=0 CONFIG_USART3_2STOP=0 +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + # # General build options # -- cgit v1.2.3 From f5f7b3f6dd1c0d48ffb0aa8c78435715a4ce3f5e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 14:42:08 +0200 Subject: Added scale ioctl to LSM303D driver --- src/drivers/lsm303d/lsm303d.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ba7316e55..af7746d14 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -705,6 +705,23 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) else return -EINVAL; + case ACCELIOCSSCALE: + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; default: /* give it to the superclass */ -- cgit v1.2.3 From eb28f5996f1e102939d9ebb3a562641137ee419b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 15:21:39 +0200 Subject: Some lost LSM303D fixes for range and scale --- src/drivers/lsm303d/lsm303d.cpp | 68 +++++++++++++++++++++++++---------------- 1 file changed, 42 insertions(+), 26 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index af7746d14..14b3d6ab8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -156,11 +156,11 @@ static const int ERROR = -1; #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) #define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) -#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6)) -#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) -#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) -#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) -#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) +#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) +#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) +#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) +#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) +#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) #define REG7_CONT_MODE_M ((0<<1) | (0<<0)) @@ -218,6 +218,8 @@ private: float _accel_range_m_s2; orb_advert_t _accel_topic; + unsigned _current_samplerate; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -405,8 +407,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : // enable debug() calls _debug_enabled = true; - _mag_range_scale = 12.0f/32767.0f; - // default scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -873,37 +873,43 @@ LSM303D::set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range = 0.0f; + float new_range_g = 0.0f; + float new_scale_g_digit = 0.0f; if (max_g == 0) max_g = 16; if (max_g <= 2) { - new_range = 2.0f; + new_range_g = 2.0f; setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range = 4.0f; + new_range_g = 4.0f; setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range = 6.0f; + new_range_g = 6.0f; setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range = 8.0f; + new_range_g = 8.0f; setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range = 16.0f; + new_range_g = 16.0f; setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; } else { return -EINVAL; } - _accel_range_m_s2 = new_range * 9.80665f; - _accel_range_scale = _accel_range_m_s2 / 32768.0f; + _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -916,6 +922,7 @@ LSM303D::mag_set_range(unsigned max_ga) uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; float new_range = 0.0f; + float new_scale_ga_digit = 0.0f; if (max_ga == 0) max_ga = 12; @@ -923,25 +930,29 @@ LSM303D::mag_set_range(unsigned max_ga) if (max_ga <= 2) { new_range = 2.0f; setbits |= REG6_FULL_SCALE_2GA_M; + new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { new_range = 4.0f; setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { new_range = 8.0f; setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { new_range = 12.0f; setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; } else { return -EINVAL; } _mag_range_ga = new_range; - _mag_range_scale = _mag_range_ga / 32768.0f; + _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1008,18 +1019,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; + _current_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; + _current_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; + _current_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; + _current_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; + _current_samplerate = 1600; } else { return -EINVAL; @@ -1358,7 +1374,7 @@ void test() { int fd_accel = -1; - struct accel_report a_report; + struct accel_report accel_report; ssize_t sz; int filter_bandwidth; @@ -1369,20 +1385,20 @@ test() err(1, "%s open failed", ACCEL_DEVICE_PATH); /* do a simple demand read */ - sz = read(fd_accel, &a_report, sizeof(a_report)); + sz = read(fd_accel, &accel_report, sizeof(accel_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(accel_report)) err(1, "immediate read failed"); - warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); - warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); - warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); - warnx("accel x: \t%d\traw", (int)a_report.x_raw); - warnx("accel y: \t%d\traw", (int)a_report.y_raw); - warnx("accel z: \t%d\traw", (int)a_report.z_raw); + warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); + warnx("accel x: \t%d\traw", (int)accel_report.x_raw); + warnx("accel y: \t%d\traw", (int)accel_report.y_raw); + warnx("accel z: \t%d\traw", (int)accel_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else -- cgit v1.2.3 From f4df4a4e081d9eaaa5dbeef013fa6320b0cea3f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 15:30:39 +0200 Subject: Forgot to add current samplerate to constructor --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 14b3d6ab8..b157d74c6 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -395,6 +395,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _current_samplerate(0), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), -- cgit v1.2.3 From da152e148d471ded29857e29040f33c7356c9050 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 16:15:43 +0200 Subject: Added iirFilter to LSM303D --- makefiles/config_px4fmu-v1_default.mk | 1 - src/drivers/lsm303d/iirFilter.c | 255 ++++++++++++++++++++++++++++++++++ src/drivers/lsm303d/iirFilter.h | 59 ++++++++ src/drivers/lsm303d/lsm303d.cpp | 32 ++++- src/drivers/lsm303d/module.mk | 3 +- 5 files changed, 345 insertions(+), 5 deletions(-) create mode 100644 src/drivers/lsm303d/iirFilter.c create mode 100644 src/drivers/lsm303d/iirFilter.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d9a699da7..74be1cd23 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -18,7 +18,6 @@ MODULES += drivers/led MODULES += drivers/px4io MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu -MODULES += drivers/lsm303d MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 MODULES += drivers/bma180 diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c new file mode 100644 index 000000000..8311f14d6 --- /dev/null +++ b/src/drivers/lsm303d/iirFilter.c @@ -0,0 +1,255 @@ +#include "math.h" +#include "string.h" +#include "iirFilter.h" + +/////////////////////////////////////////////////////////////////////////////// +// Internal function prototypes + +int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd); + +int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd); + +int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt); + +int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly); + +/////////////////////////////////////////////////////////////////////////////// +// external functions + +int testFunction() +{ + printf("TEST\n"); + return 1; +} + +int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt) +{ + TF_POLY_t polyd; + TF_ZPG_t zpgd; + + memset(pFilt, 0, sizeof(FIL_T)); + + // perform bilinear transform with frequency pre warping + btDifcToZpgd(pDifc, Ts, &zpgd); + + // calculate polynom + tZpgxToPolyx(&zpgd, &polyd); + + // set the filter parameters + tPolydToFil(&polyd, pFilt); + + return 1; +} + +// run filter using inp, return output of the filter +float updateFilter(FIL_T *pFilt, float inp) +{ + float outp = 0; + int idx; // index used for different things + int i; // loop variable + + // Store the input to the input array + idx = pFilt->inpCnt % MAX_LENGTH; + pFilt->inpData[idx] = inp; + + // calculate numData * inpData + for (i = 0; i < pFilt->numLength; i++) + { + // index of inp array + idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH; + outp += pFilt->numData[i] * pFilt->inpData[idx]; + } + + // calculate denData * outData + for (i = 0; i < pFilt->denLength; i++) + { + // index of inp array + idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH; + outp -= pFilt->denData[i] * pFilt->outData[idx]; + } + + // store the ouput data to the output array + idx = pFilt->inpCnt % MAX_LENGTH; + pFilt->outData[idx] = outp; + + pFilt->inpCnt += 1; + + return outp; +} + +/////////////////////////////////////////////////////////////////////////////// +// Internal functions + +int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt) +{ + double gain; + int i; + + if (pkPolyd->Ts < 0) + { + return 0; + } + + // initialize filter to 0 + memset(pFilt, 0, sizeof(FIL_T)); + + gain = pkPolyd->denData[pkPolyd->denLength - 1]; + pFilt->Ts = pkPolyd->Ts; + + pFilt->denLength = pkPolyd->denLength - 1; + pFilt->numLength = pkPolyd->numLength; + + for (i = 0; i < pkPolyd->numLength; i++) + { + pFilt->numData[i] = pkPolyd->numData[i] / gain; + } + + for (i = 0; i < (pkPolyd->denLength - 1); i++) + { + pFilt->denData[i] = pkPolyd->denData[i] / gain; + } +} + +// bilinear transformation of poles and zeros +int btDifcToZpgd(const TF_DIF_t *pkDifc, + double Ts, + TF_ZPG_t *pZpgd) +{ + TF_ZPG_t zpgc; + int totDiff; + int i; + + zpgc.zerosLength = 0; + zpgc.polesLength = 0; + zpgc.gain = pkDifc->gain; + zpgc.Ts = pkDifc->Ts; + + // Total number of differentiators / integerators + // positive diff, negative integ, 0 for no int/diff + totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength; + + while (zpgc.zerosLength < totDiff) + { + zpgc.zerosData[zpgc.zerosLength] = 0; + zpgc.zerosLength++; + } + while (zpgc.polesLength < -totDiff) + { + zpgc.polesData[zpgc.polesLength] = 0; + zpgc.polesLength++; + } + + // The next step is to calculate the poles + // This has to be done for the highpass and lowpass filters + // As we are using bilinear transformation there will be frequency + // warping which has to be accounted for + for (i = 0; i < pkDifc->lowpassLength; i++) + { + // pre-warping: + double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0); + // calculate pole: + zpgc.polesData[zpgc.polesLength] = -freq; + // adjust gain such that lp has gain = 1 for low frequencies + zpgc.gain *= freq; + zpgc.polesLength++; + } + for (i = 0; i < pkDifc->highpassLength; i++) + { + // pre-warping + double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0); + // calculate pole: + zpgc.polesData[zpgc.polesLength] = -freq; + // gain does not have to be adjusted + zpgc.polesLength++; + } + + return btZpgcToZpgd(&zpgc, Ts, pZpgd); +} + +// bilinear transformation of poles and zeros +int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, + double Ts, + TF_ZPG_t *pZpgd) +{ + int i; + + // init digital gain + pZpgd->gain = pkZpgc->gain; + + // transform the poles + pZpgd->polesLength = pkZpgc->polesLength; + for (i = 0; i < pkZpgc->polesLength; i++) + { + pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]); + pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]); + } + + // transform the zeros + pZpgd->zerosLength = pkZpgc->zerosLength; + for (i = 0; i < pkZpgc->zerosLength; i++) + { + pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]); + pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]); + } + + // if we don't have the same number of poles as zeros we have to add + // poles or zeros due to the bilinear transformation + while (pZpgd->zerosLength < pZpgd->polesLength) + { + pZpgd->zerosData[pZpgd->zerosLength] = -1.0; + pZpgd->zerosLength++; + } + while (pZpgd->zerosLength > pZpgd->polesLength) + { + pZpgd->polesData[pZpgd->polesLength] = -1.0; + pZpgd->polesLength++; + } + + pZpgd->Ts = Ts; + + return 1; +} + +// calculate polynom from zero, pole, gain form +int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly) +{ + int i, j; // counter variable + double tmp0, tmp1, gain; + + // copy Ts + pPoly->Ts = pkZpg->Ts; + + // calculate denominator polynom + pPoly->denLength = 1; + pPoly->denData[0] = 1.0; + for (i = 0; i < pkZpg->polesLength; i++) + { + // init temporary variable + tmp0 = 0.0; + // increase the polynom by 1 + pPoly->denData[pPoly->denLength] = 0; + pPoly->denLength++; + for (j = 0; j < pPoly->denLength; j++) + { + tmp1 = pPoly->denData[j]; + pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0; + tmp0 = tmp1; + } + } + + // calculate numerator polynom + pPoly->numLength = 1; + pPoly->numData[0] = pkZpg->gain; + for (i = 0; i < pkZpg->zerosLength; i++) + { + tmp0 = 0.0; + pPoly->numData[pPoly->numLength] = 0; + pPoly->numLength++; + for (j = 0; j < pPoly->numLength; j++) + { + tmp1 = pPoly->numData[j]; + pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0; + tmp0 = tmp1; + } + } +} diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h new file mode 100644 index 000000000..ab4223a8e --- /dev/null +++ b/src/drivers/lsm303d/iirFilter.h @@ -0,0 +1,59 @@ +#ifndef IIRFILTER__H__ +#define IIRFILTER__H__ + +__BEGIN_DECLS + +#define MAX_LENGTH 4 + +typedef struct FILTER_s +{ + float denData[MAX_LENGTH]; + float numData[MAX_LENGTH]; + int denLength; + int numLength; + float Ts; + float inpData[MAX_LENGTH]; + float outData[MAX_LENGTH]; + unsigned int inpCnt; +} FIL_T; + +typedef struct TF_ZPG_s +{ + int zerosLength; + double zerosData[MAX_LENGTH + 1]; + int polesLength; + double polesData[MAX_LENGTH + 1]; + double gain; + double Ts; +} TF_ZPG_t; + +typedef struct TF_POLY_s +{ + int numLength; + double numData[MAX_LENGTH]; + int denLength; + double denData[MAX_LENGTH]; + double Ts; +} TF_POLY_t; + +typedef struct TF_DIF_s +{ + int numInt; + int numDiff; + int lowpassLength; + int highpassLength; + double lowpassData[MAX_LENGTH]; + int highpassData[MAX_LENGTH]; + double gain; + double Ts; +} TF_DIF_t; + +__EXPORT int testFunction(void); + +__EXPORT float updateFilter(FIL_T *pFilt, float inp); + +__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt); + +__END_DECLS + +#endif diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b157d74c6..80d777826 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,6 +65,7 @@ #include #include +#include "iirFilter.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -220,6 +221,10 @@ private: unsigned _current_samplerate; + FIL_T _filter_x; + FIL_T _filter_y; + FIL_T _filter_z; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -489,6 +494,22 @@ LSM303D::init() set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ + /* initiate filter */ + TF_DIF_t tf_filter; + tf_filter.numInt = 0; + tf_filter.numDiff = 0; + tf_filter.lowpassLength = 2; + tf_filter.highpassLength = 0; + tf_filter.lowpassData[0] = 10; + tf_filter.lowpassData[1] = 10; + //tf_filter.highpassData[0] = ; + tf_filter.gain = 1; + tf_filter.Ts = 1/_current_samplerate; + + initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); + initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); + initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); @@ -1161,9 +1182,14 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; - accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + accel_report->x = updateFilter(&_filter_x, x_in_new); + accel_report->y = updateFilter(&_filter_y, y_in_new); + accel_report->z = updateFilter(&_filter_z, z_in_new); + accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index e93b1e331..8fd5679c9 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = lsm303d -SRCS = lsm303d.cpp +SRCS = lsm303d.cpp \ + iirFilter.c -- cgit v1.2.3 From e309b9ab4a633065f06a0f0c66542df84e6147d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 19:45:44 +0200 Subject: Disable IIR filter for now --- src/drivers/lsm303d/lsm303d.cpp | 52 ++++++++++++++++++++++------------------- 1 file changed, 28 insertions(+), 24 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 80d777826..a9a9bb69f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -221,9 +221,9 @@ private: unsigned _current_samplerate; - FIL_T _filter_x; - FIL_T _filter_y; - FIL_T _filter_z; +// FIL_T _filter_x; +// FIL_T _filter_y; +// FIL_T _filter_z; unsigned _num_mag_reports; volatile unsigned _next_mag_report; @@ -495,20 +495,20 @@ LSM303D::init() set_samplerate(400); /* max sample rate */ /* initiate filter */ - TF_DIF_t tf_filter; - tf_filter.numInt = 0; - tf_filter.numDiff = 0; - tf_filter.lowpassLength = 2; - tf_filter.highpassLength = 0; - tf_filter.lowpassData[0] = 10; - tf_filter.lowpassData[1] = 10; - //tf_filter.highpassData[0] = ; - tf_filter.gain = 1; - tf_filter.Ts = 1/_current_samplerate; - - initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); - initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); - initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); +// TF_DIF_t tf_filter; +// tf_filter.numInt = 0; +// tf_filter.numDiff = 0; +// tf_filter.lowpassLength = 2; +// tf_filter.highpassLength = 0; +// tf_filter.lowpassData[0] = 10; +// tf_filter.lowpassData[1] = 10; +// //tf_filter.highpassData[0] = ; +// tf_filter.gain = 1; +// tf_filter.Ts = 1/_current_samplerate; +// +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); @@ -1182,13 +1182,17 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; - float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - accel_report->x = updateFilter(&_filter_x, x_in_new); - accel_report->y = updateFilter(&_filter_y, y_in_new); - accel_report->z = updateFilter(&_filter_z, z_in_new); +// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; +// float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; +// float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; +// +// accel_report->x = updateFilter(&_filter_x, x_in_new); +// accel_report->y = updateFilter(&_filter_y, y_in_new); +// accel_report->z = updateFilter(&_filter_z, z_in_new); + + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; -- cgit v1.2.3 From 374d204b9223b5b3c33ec1fd48a120a9984160c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Jul 2013 19:48:15 +0200 Subject: Enable BDU instead of CONT mode --- src/drivers/lsm303d/lsm303d.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a9a9bb69f..844cc3884 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -125,7 +125,7 @@ static const int ERROR = -1; #define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_BDU_UPDATE (1<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) @@ -482,7 +482,7 @@ LSM303D::init() _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); /* enable accel, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); /* enable mag, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); -- cgit v1.2.3 From da1bf69ce249f22df16e7ccb21d17c08bcbbbedf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jul 2013 13:07:51 +0200 Subject: Added gyro scale estimation (but param is NOT written) --- src/modules/commander/gyro_calibration.c | 80 ++++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c index f452910c9..865f74ab4 100644 --- a/src/modules/commander/gyro_calibration.c +++ b/src/modules/commander/gyro_calibration.c @@ -104,6 +104,86 @@ void do_gyro_calibration(int mavlink_fd) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; + + + + /*** --- SCALING --- ***/ + + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; + if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) + break; + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag > M_PI_F) mag -= 2*M_PI_F; + if (mag < -M_PI_F) mag += 2*M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2*M_PI_F; + if (diff < -M_PI_F) diff += 2*M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + + warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + warnx("gyro scale: yaw (z): %6.4f", gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) -- cgit v1.2.3 From c93e743374731ec06020ba6919c6d48594d4a58c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 Jul 2013 17:47:32 +0200 Subject: Changed the MS5611 from the workq to hrt_call_every implementation, this seems to solve the SPI chip select overlaps --- src/drivers/ms5611/ms5611.cpp | 138 +++++++++++++++++------------------------- 1 file changed, 54 insertions(+), 84 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b3..452416035 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,8 +111,9 @@ protected: ms5611::prom_s _prom; - struct work_s _work; - unsigned _measure_ticks; + struct hrt_call _baro_call; + + unsigned _call_baro_interval; unsigned _num_reports; volatile unsigned _next_report; @@ -143,12 +144,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -166,12 +167,11 @@ protected: void cycle(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. + * Static trampoline; XXX is this needed? * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,6 +184,7 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); + }; /* @@ -195,7 +196,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _measure_ticks(0), + _call_baro_interval(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -212,14 +213,13 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_call_baro_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,41 +298,13 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + _measure_phase = 0; + _oldest_report = _next_report = 0; + measure(); - } while (0); + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -347,8 +319,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; + stop(); + _call_baro_interval = 0; return OK; /* external signalling not supported */ @@ -362,14 +334,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -377,7 +349,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); @@ -387,11 +359,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* update interval for next measurement */ - _measure_ticks = ticks; + _baro_call.period = _call_baro_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -399,10 +371,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_call_baro_interval == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return (1000 / _call_baro_interval); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -419,11 +391,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); + stop(); delete[] _reports; _num_reports = arg; _reports = buf; - start_cycle(); + start(); return OK; } @@ -457,30 +429,32 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start() { + /* make sure we are stopped first */ + stop(); - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; + /* reset the report ring */ _oldest_report = _next_report = 0; + /* reset to first measure phase */ + _measure_phase = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); } void -MS5611::stop_cycle() +MS5611::stop() { - work_cancel(HPWORK, &_work); + hrt_cancel(&_baro_call); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = reinterpret_cast(arg); + MS5611 *dev = (MS5611 *)arg; - dev->cycle(); + cycle(); } void @@ -504,7 +478,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } @@ -517,14 +491,16 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + // XXX maybe do something appropriate as well - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); +// /* schedule a fresh cycle call when we are ready to measure again */ +// work_queue(HPWORK, +// &_work, +// (worker_t)&MS5611::cycle_trampoline, +// this, +// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -535,19 +511,12 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } /* next phase is collection */ _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -696,13 +665,14 @@ MS5611::collect() return OK; } + void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u ticks\n", _call_baro_interval); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); -- cgit v1.2.3 From 094ff1261aa4a651e898c50d4451d283cb899a72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 Jul 2013 18:30:01 +0200 Subject: Corrected the interval of the MS5611 --- src/drivers/ms5611/ms5611.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 452416035..122cf0cec 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -336,8 +336,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); - /* set interval for next measurement to minimum legal value */ - _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); + /* set interval to minimum legal value */ + _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; + /* if we need to start the poll state machine, do it */ if (want_start) @@ -351,15 +352,12 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if (1000000/arg < MS5611_CONVERSION_INTERVAL) return -EINVAL; - /* update interval for next measurement */ - _baro_call.period = _call_baro_interval = ticks; + /* update interval */ + _baro_call.period = _call_baro_interval = 1000000/arg; /* if we need to start the poll state machine, do it */ if (want_start) -- cgit v1.2.3 From 1dac58571eadf528e949c4e170de1edf61be2982 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 19 Jul 2013 22:40:45 +0400 Subject: multirotor_pos_control: minor cleanup --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e5a3f3e54..b49186ee9 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -95,7 +95,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); exit(1); } @@ -314,7 +314,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) double lat_home = local_pos.home_lat * 1e-7; double lon_home = local_pos.home_lon * 1e-7; map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); + warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); } -- cgit v1.2.3 From 68ab69de010adbe37b37558824ca013ecd0d569a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:47:51 +0200 Subject: moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results --- src/modules/commander/accelerometer_calibration.c | 453 ------ .../commander/accelerometer_calibration.cpp | 464 ++++++ src/modules/commander/airspeed_calibration.c | 111 -- src/modules/commander/airspeed_calibration.cpp | 113 ++ src/modules/commander/baro_calibration.c | 54 - src/modules/commander/baro_calibration.cpp | 54 + src/modules/commander/calibration_routines.c | 220 --- src/modules/commander/calibration_routines.cpp | 220 +++ src/modules/commander/commander.c | 1721 -------------------- src/modules/commander/commander.cpp | 1720 +++++++++++++++++++ src/modules/commander/commander.h | 54 - src/modules/commander/commander_helper.c | 218 --- src/modules/commander/commander_helper.cpp | 219 +++ src/modules/commander/commander_params.c | 54 + src/modules/commander/gyro_calibration.c | 231 --- src/modules/commander/gyro_calibration.cpp | 279 ++++ src/modules/commander/mag_calibration.c | 278 ---- src/modules/commander/mag_calibration.cpp | 280 ++++ src/modules/commander/module.mk | 21 +- src/modules/commander/rc_calibration.c | 83 - src/modules/commander/rc_calibration.cpp | 83 + src/modules/commander/state_machine_helper.c | 758 --------- src/modules/commander/state_machine_helper.cpp | 763 +++++++++ src/modules/systemlib/systemlib.h | 7 +- 24 files changed, 4264 insertions(+), 4194 deletions(-) delete mode 100644 src/modules/commander/accelerometer_calibration.c create mode 100644 src/modules/commander/accelerometer_calibration.cpp delete mode 100644 src/modules/commander/airspeed_calibration.c create mode 100644 src/modules/commander/airspeed_calibration.cpp delete mode 100644 src/modules/commander/baro_calibration.c create mode 100644 src/modules/commander/baro_calibration.cpp delete mode 100644 src/modules/commander/calibration_routines.c create mode 100644 src/modules/commander/calibration_routines.cpp delete mode 100644 src/modules/commander/commander.c create mode 100644 src/modules/commander/commander.cpp delete mode 100644 src/modules/commander/commander.h delete mode 100644 src/modules/commander/commander_helper.c create mode 100644 src/modules/commander/commander_helper.cpp create mode 100644 src/modules/commander/commander_params.c delete mode 100644 src/modules/commander/gyro_calibration.c create mode 100644 src/modules/commander/gyro_calibration.cpp delete mode 100644 src/modules/commander/mag_calibration.c create mode 100644 src/modules/commander/mag_calibration.cpp delete mode 100644 src/modules/commander/rc_calibration.c create mode 100644 src/modules/commander/rc_calibration.cpp delete mode 100644 src/modules/commander/state_machine_helper.c create mode 100644 src/modules/commander/state_machine_helper.cpp diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c deleted file mode 100644 index bc9d24956..000000000 --- a/src/modules/commander/accelerometer_calibration.c +++ /dev/null @@ -1,453 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 accelerometer_calibration.c - * - * Implementation of accelerometer calibration. - * - * Transform acceleration vector to true orientation, scale and offset - * - * ===== Model ===== - * accel_corr = accel_T * (accel_raw - accel_offs) - * - * accel_corr[3] - fully corrected acceleration vector in body frame - * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform - * accel_raw[3] - raw acceleration vector - * accel_offs[3] - acceleration offset vector - * - * ===== Calibration ===== - * - * Reference vectors - * accel_corr_ref[6][3] = [ g 0 0 ] // nose up - * | -g 0 0 | // nose down - * | 0 g 0 | // left side down - * | 0 -g 0 | // right side down - * | 0 0 g | // on back - * [ 0 0 -g ] // level - * accel_raw_ref[6][3] - * - * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 - * - * 6 reference vectors * 3 axes = 18 equations - * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants - * - * Find accel_offs - * - * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 - * - * Find accel_T - * - * 9 unknown constants - * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations - * - * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 - * - * Solve separate system for each row of accel_T: - * - * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 - * - * A * x = b - * - * x = [ accel_T[0][i] ] - * | accel_T[1][i] | - * [ accel_T[2][i] ] - * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough - * | accel_corr_ref[2][i] | - * [ accel_corr_ref[4][i] ] - * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 - * - * Matrix A is common for all three systems: - * A = [ a[0][0] a[0][1] a[0][2] ] - * | a[2][0] a[2][1] a[2][2] | - * [ a[4][0] a[4][1] a[4][2] ] - * - * x = A^-1 * b - * - * accel_T = A^-1 * g - * g = 9.80665 - * - * @author Anton Babushkin - */ - -#include "accelerometer_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include - - -#include -#include -#include -#include -#include -#include -#include - -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); -int detect_orientation(int mavlink_fd, int sub_sensor_combined); -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); -int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); - -void do_accel_calibration(int mavlink_fd) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - - /* measure and calculate offsets & scales */ - float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); - - if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); - } - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_positive(); - } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_negative(); - } - - /* exit accel calibration mode */ -} - -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { - const int samples_num = 2500; - float accel_ref[6][3]; - bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - while (true) { - bool done = true; - char str[80]; - int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); - for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); - done = false; - } - } - if (done) - break; - mavlink_log_info(mavlink_fd, str); - - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) - return ERROR; - - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); - read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); - data_collected[orient] = true; - tune_neutral(); - } - close(sensor_combined_sub); - - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); - return ERROR; - } - - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; - } - - return OK; -} - -/* - * Wait for vehicle become still and detect it's orientation. - * - * @return 0..5 according to orientation when vehicle is still and ready for measurements, - * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 - */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { - struct sensor_combined_s sensor; - /* exponential moving average of accel */ - float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; - /* max-hold dispersion of accel */ - float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - /* EMA time constant in seconds*/ - float ema_len = 0.2f; - /* set "still" threshold to 0.1 m/s^2 */ - float still_thr2 = pow(0.1f, 2); - /* set accel error threshold to 5m/s^2 */ - float accel_err_thr = 5.0f; - /* still time required in us */ - int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - hrt_abstime t_start = hrt_absolute_time(); - /* set timeout to 30s */ - hrt_abstime timeout = 30000000; - hrt_abstime t_timeout = t_start + timeout; - hrt_abstime t = t_start; - hrt_abstime t_prev = t_start; - hrt_abstime t_still = 0; - - unsigned poll_errcount = 0; - - while (true) { - /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); - t = hrt_absolute_time(); - float dt = (t - t_prev) / 1000000.0f; - t_prev = t; - float w = dt / ema_len; - for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; - d = d * d; - accel_disp[i] = accel_disp[i] * (1.0f - w); - if (d > accel_disp[i]) - accel_disp[i] = d; - } - /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { - /* is still now */ - if (t_still == 0) { - /* first time */ - mavlink_log_info(mavlink_fd, "still..."); - t_still = t; - t_timeout = t + timeout; - } else { - /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { - /* vehicle is still, exit from the loop to detection of its orientation */ - break; - } - } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { - /* not still, reset still start time */ - if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); - t_still = 0; - } - } - } else if (poll_ret == 0) { - poll_errcount++; - } - if (t > t_timeout) { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); - return -1; - } - } - - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) - return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) - return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) - return 5; // [ 0, 0, -g ] - - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); - - return -2; // Can't detect orientation -} - -/* - * Read specified number of accelerometer samples, calculate average and dispersion. - */ -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; - int count = 0; - float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; - - while (count < samples_num) { - int poll_ret = poll(fds, 1, 1000); - if (poll_ret == 1) { - struct sensor_combined_s sensor; - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) - accel_sum[i] += sensor.accelerometer_m_s2[i]; - count++; - } else { - return ERROR; - } - } - - for (int i = 0; i < 3; i++) { - accel_avg[i] = accel_sum[i] / count; - } - - return OK; -} - -int mat_invert3(float src[3][3], float dst[3][3]) { - float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) - return ERROR; // Singular matrix - - dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; - dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; - dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; - dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; - dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; - dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; - dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; - dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; - dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - - return OK; -} - -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { - /* calculate offsets */ - for (int i = 0; i < 3; i++) { - accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; - } - - /* fill matrix A for linear equations system*/ - float mat_A[3][3]; - memset(mat_A, 0, sizeof(mat_A)); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - float a = accel_ref[i * 2][j] - accel_offs[j]; - mat_A[i][j] = a; - } - } - - /* calculate inverse matrix for A */ - float mat_A_inv[3][3]; - if (mat_invert3(mat_A, mat_A_inv) != OK) - return ERROR; - - /* copy results to accel_T */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - /* simplify matrices mult because b has only one non-zero element == g at index i */ - accel_T[j][i] = mat_A_inv[j][i] * g; - } - } - - return OK; -} diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp new file mode 100644 index 000000000..df92d51d2 --- /dev/null +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -0,0 +1,464 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 accelerometer_calibration.cpp + * + * Implementation of accelerometer calibration. + * + * Transform acceleration vector to true orientation, scale and offset + * + * ===== Model ===== + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in body frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - acceleration offset vector + * + * ===== Calibration ===== + * + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // nose up + * | -g 0 0 | // nose down + * | 0 g 0 | // left side down + * | 0 -g 0 | // right side down + * | 0 0 g | // on back + * [ 0 0 -g ] // level + * accel_raw_ref[6][3] + * + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 + * + * 6 reference vectors * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * + * A * x = b + * + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] + * + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 + * + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] + * + * x = A^-1 * b + * + * accel_T = A^-1 * g + * g = 9.80665 + * + * @author Anton Babushkin + */ + +#include "accelerometer_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); + +void do_accel_calibration(int mavlink_fd) { + /* announce change */ + mavlink_log_info(mavlink_fd, "accel calibration started"); + + /* measure and calculate offsets & scales */ + float accel_offs[3]; + float accel_scale[3]; + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + + if (res == OK) { + /* measurements complete successfully, set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + } + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offs[0], + accel_scale[0], + accel_offs[1], + accel_scale[1], + accel_offs[2], + accel_scale[2], + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + mavlink_log_info(mavlink_fd, "accel calibration done"); + tune_positive(); + } else { + /* measurements error */ + mavlink_log_info(mavlink_fd, "accel calibration aborted"); + tune_negative(); + } + + /* exit accel calibration mode */ +} + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { + const int samples_num = 2500; + float accel_ref[6][3]; + bool data_collected[6] = { false, false, false, false, false, false }; + const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + + /* reset existing calibration */ + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); + close(fd); + + if (OK != ioctl_res) { + warn("ERROR: failed to set scale / offsets for accel"); + return ERROR; + } + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + while (true) { + bool done = true; + char str[80]; + int str_ptr; + str_ptr = sprintf(str, "keep vehicle still:"); + for (int i = 0; i < 6; i++) { + if (!data_collected[i]) { + str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); + done = false; + } + } + if (done) + break; + mavlink_log_info(mavlink_fd, str); + + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) + return ERROR; + + sprintf(str, "meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_neutral(); + } + close(sensor_combined_sub); + + /* calculate offsets and rotation+scale matrix */ + float accel_T[3][3]; + int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + return ERROR; + } + + /* convert accel transform matrix to scales, + * rotation part of transform matrix is not used by now + */ + for (int i = 0; i < 3; i++) { + accel_scale[i] = accel_T[i][i]; + } + + return OK; +} + +/* + * Wait for vehicle become still and detect it's orientation. + * + * @return 0..5 according to orientation when vehicle is still and ready for measurements, + * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 + */ +int detect_orientation(int mavlink_fd, int sub_sensor_combined) { + struct sensor_combined_s sensor; + /* exponential moving average of accel */ + float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; + /* max-hold dispersion of accel */ + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; + /* EMA time constant in seconds*/ + float ema_len = 0.2f; + /* set "still" threshold to 0.1 m/s^2 */ + float still_thr2 = pow(0.1f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; + /* still time required in us */ + int64_t still_time = 2000000; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + hrt_abstime t_start = hrt_absolute_time(); + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + for (int i = 0; i < 3; i++) { + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; + float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) + accel_disp[i] = d; + } + /* still detector with hysteresis */ + if ( accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2 ) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_log_info(mavlink_fd, "still..."); + t_still = t; + t_timeout = t + timeout; + } else { + /* still since t_still */ + if ((int64_t) t - (int64_t) t_still > still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + } else if ( accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_log_info(mavlink_fd, "moving..."); + t_still = 0; + } + } + } else if (poll_ret == 0) { + poll_errcount++; + } + if (t > t_timeout) { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); + return -1; + } + } + + if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 0; // [ g, 0, 0 ] + if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 1; // [ -g, 0, 0 ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 2; // [ 0, g, 0 ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) + return 3; // [ 0, -g, 0 ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + return 4; // [ 0, 0, g ] + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + return 5; // [ 0, 0, -g ] + + mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + + return -2; // Can't detect orientation +} + +/* + * Read specified number of accelerometer samples, calculate average and dispersion. + */ +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; + int count = 0; + float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + + while (count < samples_num) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) + accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { + return ERROR; + } + } + + for (int i = 0; i < 3; i++) { + accel_avg[i] = accel_sum[i] / count; + } + + return OK; +} + +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0f) + return ERROR; // Singular matrix + + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + + return OK; +} + +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { + /* calculate offsets */ + for (int i = 0; i < 3; i++) { + accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; + } + + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = accel_ref[i * 2][j] - accel_offs[j]; + mat_A[i][j] = a; + } + } + + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) + return ERROR; + + /* copy results to accel_T */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + + return OK; +} diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c deleted file mode 100644 index feaf11aee..000000000 --- a/src/modules/commander/airspeed_calibration.c +++ /dev/null @@ -1,111 +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 airspeed_calibration.c - * Airspeed sensor calibration routine - */ - -#include "airspeed_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -void do_airspeed_calibration(int mavlink_fd) -{ - /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_positive(); - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - close(diff_pres_sub); -} diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp new file mode 100644 index 000000000..df08292e3 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 airspeed_calibration.cpp + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c deleted file mode 100644 index a70594794..000000000 --- a/src/modules/commander/baro_calibration.c +++ /dev/null @@ -1,54 +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 baro_calibration.c - * Barometer calibration routine - */ - -#include "baro_calibration.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -void do_baro_calibration(int mavlink_fd) -{ - // TODO implement this - return; -} diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp new file mode 100644 index 000000000..d7515b3d9 --- /dev/null +++ b/src/modules/commander/baro_calibration.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 baro_calibration.cpp + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +void do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return; +} diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c deleted file mode 100644 index 799cd4269..000000000 --- a/src/modules/commander/calibration_routines.c +++ /dev/null @@ -1,220 +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 calibration_routines.c - * Calibration routines implementations. - * - * @author Lorenz Meier - */ - -#include - -#include "calibration_routines.h" - - -int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) -{ - - float x_sumplain = 0.0f; - float x_sumsq = 0.0f; - float x_sumcube = 0.0f; - - float y_sumplain = 0.0f; - float y_sumsq = 0.0f; - float y_sumcube = 0.0f; - - float z_sumplain = 0.0f; - float z_sumsq = 0.0f; - float z_sumcube = 0.0f; - - float xy_sum = 0.0f; - float xz_sum = 0.0f; - float yz_sum = 0.0f; - - float x2y_sum = 0.0f; - float x2z_sum = 0.0f; - float y2x_sum = 0.0f; - float y2z_sum = 0.0f; - float z2x_sum = 0.0f; - float z2y_sum = 0.0f; - - for (unsigned int i = 0; i < size; i++) { - - float x2 = x[i] * x[i]; - float y2 = y[i] * y[i]; - float z2 = z[i] * z[i]; - - x_sumplain += x[i]; - x_sumsq += x2; - x_sumcube += x2 * x[i]; - - y_sumplain += y[i]; - y_sumsq += y2; - y_sumcube += y2 * y[i]; - - z_sumplain += z[i]; - z_sumsq += z2; - z_sumcube += z2 * z[i]; - - xy_sum += x[i] * y[i]; - xz_sum += x[i] * z[i]; - yz_sum += y[i] * z[i]; - - x2y_sum += x2 * y[i]; - x2z_sum += x2 * z[i]; - - y2x_sum += y2 * x[i]; - y2z_sum += y2 * z[i]; - - z2x_sum += z2 * x[i]; - z2y_sum += z2 * y[i]; - } - - // - //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data - // - // P is a structure that has been computed with the data earlier. - // P.npoints is the number of elements; the length of X,Y,Z are identical. - // P's members are logically named. - // - // X[n] is the x component of point n - // Y[n] is the y component of point n - // Z[n] is the z component of point n - // - // A is the x coordiante of the sphere - // B is the y coordiante of the sphere - // C is the z coordiante of the sphere - // Rsq is the radius squared of the sphere. - // - //This method should converge; maybe 5-100 iterations or more. - // - float x_sum = x_sumplain / size; //sum( X[n] ) - float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) - float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) - float y_sum = y_sumplain / size; //sum( Y[n] ) - float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) - float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) - float z_sum = z_sumplain / size; //sum( Z[n] ) - float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) - float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) - - float XY = xy_sum / size; //sum( X[n] * Y[n] ) - float XZ = xz_sum / size; //sum( X[n] * Z[n] ) - float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) - float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) - float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) - float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) - float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) - float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) - float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) - - //Reduction of multiplications - float F0 = x_sum2 + y_sum2 + z_sum2; - float F1 = 0.5f * F0; - float F2 = -8.0f * (x_sum3 + Y2X + Z2X); - float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); - float F4 = -8.0f * (X2Z + Y2Z + z_sum3); - - //Set initial conditions: - float A = x_sum; - float B = y_sum; - float C = z_sum; - - //First iteration computation: - float A2 = A * A; - float B2 = B * B; - float C2 = C * C; - float QS = A2 + B2 + C2; - float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - - //Set initial conditions: - float Rsq = F0 + QB + QS; - - //First iteration computation: - float Q0 = 0.5f * (QS - Rsq); - float Q1 = F1 + Q0; - float Q2 = 8.0f * (QS - Rsq + QB + F0); - float aA, aB, aC, nA, nB, nC, dA, dB, dC; - - //Iterate N times, ignore stop condition. - int n = 0; - - while (n < max_iterations) { - n++; - - //Compute denominator: - aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); - aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); - aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; - - //Compute next iteration - nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); - nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); - nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); - - //Check for stop condition - dA = (nA - A); - dB = (nB - B); - dC = (nC - C); - - if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } - - //Compute next iteration's values - A = nA; - B = nB; - C = nC; - A2 = A * A; - B2 = B * B; - C2 = C * C; - QS = A2 + B2 + C2; - QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); - Rsq = F0 + QB + QS; - Q0 = 0.5f * (QS - Rsq); - Q1 = F1 + Q0; - Q2 = 8.0f * (QS - Rsq + QB + F0); - } - - *sphere_x = A; - *sphere_y = B; - *sphere_z = C; - *sphere_radius = sqrtf(Rsq); - - return 0; -} - diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp new file mode 100644 index 000000000..be38ea104 --- /dev/null +++ b/src/modules/commander/calibration_routines.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * 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 calibration_routines.cpp + * Calibration routines implementations. + * + * @author Lorenz Meier + */ + +#include + +#include "calibration_routines.h" + + +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) +{ + + float x_sumplain = 0.0f; + float x_sumsq = 0.0f; + float x_sumcube = 0.0f; + + float y_sumplain = 0.0f; + float y_sumsq = 0.0f; + float y_sumcube = 0.0f; + + float z_sumplain = 0.0f; + float z_sumsq = 0.0f; + float z_sumcube = 0.0f; + + float xy_sum = 0.0f; + float xz_sum = 0.0f; + float yz_sum = 0.0f; + + float x2y_sum = 0.0f; + float x2z_sum = 0.0f; + float y2x_sum = 0.0f; + float y2z_sum = 0.0f; + float z2x_sum = 0.0f; + float z2y_sum = 0.0f; + + for (unsigned int i = 0; i < size; i++) { + + float x2 = x[i] * x[i]; + float y2 = y[i] * y[i]; + float z2 = z[i] * z[i]; + + x_sumplain += x[i]; + x_sumsq += x2; + x_sumcube += x2 * x[i]; + + y_sumplain += y[i]; + y_sumsq += y2; + y_sumcube += y2 * y[i]; + + z_sumplain += z[i]; + z_sumsq += z2; + z_sumcube += z2 * z[i]; + + xy_sum += x[i] * y[i]; + xz_sum += x[i] * z[i]; + yz_sum += y[i] * z[i]; + + x2y_sum += x2 * y[i]; + x2z_sum += x2 * z[i]; + + y2x_sum += y2 * x[i]; + y2z_sum += y2 * z[i]; + + z2x_sum += z2 * x[i]; + z2y_sum += z2 * y[i]; + } + + // + //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data + // + // P is a structure that has been computed with the data earlier. + // P.npoints is the number of elements; the length of X,Y,Z are identical. + // P's members are logically named. + // + // X[n] is the x component of point n + // Y[n] is the y component of point n + // Z[n] is the z component of point n + // + // A is the x coordiante of the sphere + // B is the y coordiante of the sphere + // C is the z coordiante of the sphere + // Rsq is the radius squared of the sphere. + // + //This method should converge; maybe 5-100 iterations or more. + // + float x_sum = x_sumplain / size; //sum( X[n] ) + float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) + float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) + float y_sum = y_sumplain / size; //sum( Y[n] ) + float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) + float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) + float z_sum = z_sumplain / size; //sum( Z[n] ) + float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) + float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) + + float XY = xy_sum / size; //sum( X[n] * Y[n] ) + float XZ = xz_sum / size; //sum( X[n] * Z[n] ) + float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) + float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) + float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) + float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) + float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) + float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) + float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) + + //Reduction of multiplications + float F0 = x_sum2 + y_sum2 + z_sum2; + float F1 = 0.5f * F0; + float F2 = -8.0f * (x_sum3 + Y2X + Z2X); + float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); + float F4 = -8.0f * (X2Z + Y2Z + z_sum3); + + //Set initial conditions: + float A = x_sum; + float B = y_sum; + float C = z_sum; + + //First iteration computation: + float A2 = A * A; + float B2 = B * B; + float C2 = C * C; + float QS = A2 + B2 + C2; + float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + + //Set initial conditions: + float Rsq = F0 + QB + QS; + + //First iteration computation: + float Q0 = 0.5f * (QS - Rsq); + float Q1 = F1 + Q0; + float Q2 = 8.0f * (QS - Rsq + QB + F0); + float aA, aB, aC, nA, nB, nC, dA, dB, dC; + + //Iterate N times, ignore stop condition. + int n = 0; + + while (n < max_iterations) { + n++; + + //Compute denominator: + aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); + aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); + aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); + aA = (aA == 0.0f) ? 1.0f : aA; + aB = (aB == 0.0f) ? 1.0f : aB; + aC = (aC == 0.0f) ? 1.0f : aC; + + //Compute next iteration + nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); + nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); + nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); + + //Check for stop condition + dA = (nA - A); + dB = (nB - B); + dC = (nC - C); + + if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } + + //Compute next iteration's values + A = nA; + B = nB; + C = nC; + A2 = A * A; + B2 = B * B; + C2 = C * C; + QS = A2 + B2 + C2; + QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + Rsq = F0 + QB + QS; + Q0 = 0.5f * (QS - Rsq); + Q1 = F1 + Q0; + Q2 = 8.0f * (QS - Rsq + QB + F0); + } + + *sphere_x = A; + *sphere_y = B; + *sphere_z = C; + *sphere_radius = sqrtf(Rsq); + + return 0; +} + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c deleted file mode 100644 index c4dc495f6..000000000 --- a/src/modules/commander/commander.c +++ /dev/null @@ -1,1721 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * 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 commander.c - * Main system state machine implementation. - * - */ - -#include "commander.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "commander_helper.h" -#include "state_machine_helper.h" -#include "calibration_routines.h" -#include "accelerometer_calibration.h" -#include "gyro_calibration.h" -#include "mag_calibration.h" -#include "baro_calibration.h" -#include "rc_calibration.h" -#include "airspeed_calibration.h" - -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - - -extern struct system_load_s system_load; - -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - -/* Decouple update interval and hysteris counters, all depends on intervals */ -#define COMMANDER_MONITORING_INTERVAL 50000 -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ - -/* Mavlink file descriptors */ -static int mavlink_fd; - -/* flags */ -static bool commander_initialized = false; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -/* timout until lowlevel failsafe */ -static unsigned int failsafe_lowlevel_timeout_ms; - -/* tasks waiting for low prio thread */ -enum { - LOW_PRIO_TASK_NONE = 0, - LOW_PRIO_TASK_PARAM_SAVE, - LOW_PRIO_TASK_PARAM_LOAD, - LOW_PRIO_TASK_GYRO_CALIBRATION, - LOW_PRIO_TASK_MAG_CALIBRATION, - LOW_PRIO_TASK_ALTITUDE_CALIBRATION, - LOW_PRIO_TASK_RC_CALIBRATION, - LOW_PRIO_TASK_ACCEL_CALIBRATION, - LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task; - - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -__EXPORT int commander_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -void usage(const char *reason); - -/** - * React to commands that are sent e.g. from the mavlink module. - */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); - -/** - * Mainloop of commander. - */ -int commander_thread_main(int argc, char *argv[]); - -/** - * Loop that runs at a lower rate and priority for calibration and parameter tasks. - */ -void *commander_low_prio_loop(void *arg); - - -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* request to set different system mode */ - switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: - - /* request to activate HIL */ - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - - if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - /* request to disarm */ - } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - - break; - - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - - /* request for an autopilot reboot */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { - /* reboot is executed later, after positive tune is sent */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - tune_positive(); - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - tune_negative(); - } - } - } - break; - - // /* request to land */ - // case VEHICLE_CMD_NAV_LAND: - // { - // //TODO: add check if landing possible - // //TODO: add landing maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } } - // break; - // - // /* request to takeoff */ - // case VEHICLE_CMD_NAV_TAKEOFF: - // { - // //TODO: add check if takeoff possible - // //TODO: add takeoff maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } - // } - // break; - // - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param1)) == 0) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; - - default: - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - break; - } - - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED || - result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - - tune_negative(); - - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - - tune_positive(); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0) { - /* send acknowledge command */ - // XXX TODO - } - -} - -int commander_thread_main(int argc, char *argv[]) -{ - /* not yet initialized */ - commander_initialized = false; - bool home_position_set = false; - - bool battery_tune_played = false; - bool arm_tune_played = false; - - /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - - /* welcome user */ - warnx("[commander] starting"); - - /* pthread for slow low prio thread */ - pthread_t commander_low_prio_thread; - - /* initialize */ - if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); - } - - if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); - } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); - } - - /* Main state machine */ - struct vehicle_status_s current_status; - orb_advert_t status_pub; - /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); - - /* armed topic */ - struct actuator_armed_s armed; - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - orb_advert_t control_mode_pub; - - /* Initialize all flags to false */ - memset(&control_mode, 0, sizeof(control_mode)); - - current_status.navigation_state = NAVIGATION_STATE_INIT; - current_status.arming_state = ARMING_STATE_INIT; - current_status.hil_state = HIL_STATE_OFF; - - /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; - - /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; - - /* allow manual override initially */ - control_mode.flag_external_manual_override_ok = true; - - /* flag position info as bad, do not allow auto mode */ - // current_status.flag_vector_flight_mode_ok = false; - - /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - - /* set safety device detection flag */ - /* XXX do we need this? */ - //current_status.flag_safety_present = false; - - // XXX for now just set sensors as initialized - current_status.condition_system_sensors_initialized = true; - - // XXX just disable offboard control for now - control_mode.flag_control_offboard_enabled = false; - - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); - /* publish current state machine */ - - /* publish the new state */ - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - - - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - - /* home position */ - orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); - - if (status_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } - - // XXX needed? - mavlink_log_info(mavlink_fd, "system is running"); - - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); - - struct sched_param param; - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - - /* Start monitoring loop */ - unsigned counter = 0; - unsigned low_voltage_counter = 0; - unsigned critical_voltage_counter = 0; - unsigned stick_off_counter = 0; - unsigned stick_on_counter = 0; - - /* To remember when last notification was sent */ - uint64_t last_print_time = 0; - - float voltage_previous = 0.0f; - - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; - - uint64_t last_idle_time = 0; - - uint64_t start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; - - bool new_data = false; - - /* Subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; - memset(&safety, 0, sizeof(safety)); - - /* Subscribe to manual control data */ - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; - memset(&sp_man, 0, sizeof(sp_man)); - - /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); - - /* Subscribe to global position */ - int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_s global_position; - memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; - - /* Subscribe to local position data */ - int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; - - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - - /* Subscribe to GPS topic */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - - /* Subscribe to sensor topic */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - - /* Subscribe to differential pressure topic */ - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - - /* Subscribe to parameters changed topic */ - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); - - /* Subscribe to battery topic */ - int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; - - /* Subscribe to subsystem info topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - memset(&info, 0, sizeof(info)); - - /* now initialized */ - commander_initialized = true; - thread_running = true; - - start_time = hrt_absolute_time(); - - while (!thread_should_exit) { - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - /* update parameters */ - if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed getting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - control_mode.flag_external_manual_override_ok = false; - - } else { - control_mode.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } - } - - orb_check(sp_man_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } - - orb_check(sp_offboard_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); - } - - orb_check(sensor_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - } - - orb_check(diff_pres_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; - } - - orb_check(cmd_sub, &new_data); - - if (new_data) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); - } - - /* update safety topic */ - orb_check(safety_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(safety), safety_sub, &safety); - } - - /* update global position estimate */ - orb_check(global_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; - } - - /* update local position estimate */ - orb_check(local_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; - } - - /* set the condition to valid if there has recently been a local position estimate */ - if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - current_status.condition_local_position_valid = true; - } else { - current_status.condition_local_position_valid = false; - } - - /* update battery status */ - orb_check(battery_sub, &new_data); - if (new_data) { - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - current_status.battery_voltage = battery.voltage_v; - current_status.condition_battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - - } - - if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { - current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); - } else { - current_status.battery_voltage = 0.0f; - } - - /* update subsystem */ - orb_check(subsys_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - current_status.onboard_control_sensors_present |= info.subsystem_type; - - } else { - current_status.onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - current_status.onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - current_status.onboard_control_sensors_health |= info.subsystem_type; - - } else { - current_status.onboard_control_sensors_health &= ~info.subsystem_type; - } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - - /* XXX if armed */ - if (armed.armed) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { - /* ready to arm */ - led_toggle(LED_AMBER); - } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not ready to arm, something is wrong */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - - // /* toggle GPS led at 5 Hz in HIL mode */ - // if (current_status.flag_hil_enabled) { - // /* hil enabled */ - // led_toggle(LED_BLUE); - - // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - // /* toggle arming (red) at 5 Hz on low battery or error */ - // led_toggle(LED_AMBER); - // } - - } - - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } - - - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - tune_low_bat(); - } - - low_voltage_counter++; - } - - /* Critical, this is rather an emergency, change state machine */ - else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - tune_critical_bat(); - // XXX implement state change here - } - - critical_voltage_counter++; - - } else { - low_voltage_counter = 0; - critical_voltage_counter = 0; - } - - /* End battery voltage check */ - - /* If in INIT state, try to proceed to STANDBY state */ - if (current_status.arming_state == ARMING_STATE_INIT) { - // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - } else { - // XXX: Add emergency stuff if sensors are lost - } - - - /* - * Check for valid position information. - * - * If the system has a valid position source from an onboard - * position estimator, it is safe to operate it autonomously. - * The flag_vector_flight_mode_ok flag indicates that a minimum - * set of position measurements is available. - */ - - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.condition_global_position_valid; - bool local_pos_valid = current_status.condition_local_position_valid; - bool airspeed_valid = current_status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_global_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.condition_global_position_valid = false; - } - - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_local_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_airspeed_valid = true; - - } else { - current_status.condition_airspeed_valid = false; - } - - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - // if (current_status.condition_local_position_valid || - // current_status.condition_global_position_valid) { - // current_status.flag_vector_flight_mode_ok = true; - - // } else { - // current_status.flag_vector_flight_mode_ok = false; - // } - - /* consolidate state change, flag as changed if required */ - if (global_pos_valid != current_status.condition_global_position_valid || - local_pos_valid != current_status.condition_local_position_valid || - airspeed_valid != current_status.condition_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - // if (!current_status.flag_valid_launch_position && - // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it - - // // XXX implement storage and publication of RTL position - // current_status.flag_valid_launch_position = true; - // current_status.flag_auto_flight_mode_ok = true; - // state_changed = true; - // } - - orb_check(gps_sub, &new_data); - if (new_data) { - - - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - - /* check if gps fix is ok */ - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !armed.armed) { - warnx("setting home position"); - - /* copy position data to uORB home message, store it locally as well */ - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - home_position_set = true; - tune_positive(); - } - } - - /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - - warnx("mode sw not finite"); - /* no valid stick position, go to default */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, go to manual mode */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.mode_switch = MODE_SWITCH_AUTO; - - } else { - - /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_SEATBELT; - } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if land/RTL is requested - */ - if (!isfinite(sp_man.return_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.return_switch = RETURN_SWITCH_RETURN; - - } else { - /* center stick position, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - } - - /* check mission switch */ - if (!isfinite(sp_man.mission_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { - - /* top switch position */ - current_status.mission_switch = MISSION_SWITCH_MISSION; - - } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom switch position */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else { - - /* center switch position, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? - } - - /* Now it's time to handle the stick inputs */ - - switch (current_status.arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* just manual, XXX this might depend on the return switch */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - - /* Try seatbelt or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - - /* Try auto or fallback to seatbelt or even manual */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - /* Always accept manual mode */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - - /* SEATBELT_STANDBY (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_NONE) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* SEATBELT_DESCENT (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_RETURN) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_NONE) { - - /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - - /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_MISSION) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - - /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_RETURN - && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - break; - - // XXX we might be missing something that triggers a transition from RTL to LAND - - case ARMING_STATE_ARMED_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - } - - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } - stick_off_counter = 0; - - } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - stick_on_counter = 0; - tune_positive(); - - } else { - stick_on_counter++; - stick_off_counter = 0; - } - } - - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; - - } else { - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - - if (!current_status.rc_signal_cutting_off) { - printf("Reason: not rc_signal_cutting_off\n"); - } else { - printf("last print time: %llu\n", last_print_time); - } - - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; - state_changed = true; - } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } - } - } - - - - - /* End mode switch */ - - /* END RC state check */ - - - /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - - // /* decide about attitude control flag, enable in att/pos/vel */ - // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - // /* decide about rate control flag, enable it always XXX (for now) */ - // bool rates_ctrl_enabled = true; - - // /* set up control mode */ - // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - // state_changed = true; - // } - - // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - // current_status.flag_control_rates_enabled = rates_ctrl_enabled; - // state_changed = true; - // } - - // /* handle the case where offboard control signal was regained */ - // if (!current_status.offboard_control_signal_found_once) { - // current_status.offboard_control_signal_found_once = true; - // /* enable offboard control, disable manual input */ - // current_status.flag_control_manual_enabled = false; - // current_status.flag_control_offboard_enabled = true; - // state_changed = true; - // tune_positive(); - - // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - // } else { - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - // state_changed = true; - // tune_positive(); - // } - // } - - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; - - // XXX check if this is correct - /* arm / disarm on request */ - if (sp_offboard.armed && !armed.armed) { - - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - - } else if (!sp_offboard.armed && armed.armed) { - - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - } - - } else { - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_positive(); - - /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; - state_changed = true; - } - } - } - } - - - - - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - - - // XXX this is missing - /* If full run came back clean, transition to standby */ - // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - // current_status.flag_preflight_gyro_calibration == false && - // current_status.flag_preflight_mag_calibration == false && - // current_status.flag_preflight_accel_calibration == false) { - // /* All ok, no calibration going on, go to standby */ - // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - // } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - state_changed = false; - } - - - - /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.battery_voltage; - - - /* play tone according to evaluation result */ - /* check if we recently armed */ - if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - if (tune_arm() == OK) - arm_tune_played = true; - - /* Trigger audio event for low battery */ - } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { - if (tune_critical_bat() == OK) - battery_tune_played = true; - } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { - if (tune_low_bat() == OK) - battery_tune_played = true; - } else if(battery_tune_played) { - tune_stop(); - battery_tune_played = false; - } - - /* reset arm_tune_played when disarmed */ - if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { - arm_tune_played = false; - } - - - /* XXX use this voltage_previous */ - fflush(stdout); - counter++; - usleep(COMMANDER_MONITORING_INTERVAL); - } - - /* wait for threads to complete */ - pthread_join(commander_low_prio_thread, NULL); - - /* close fds */ - led_deinit(); - buzzer_deinit(); - close(sp_man_sub); - close(sp_offboard_sub); - close(local_position_sub); - close(global_position_sub); - close(gps_sub); - close(sensor_sub); - close(safety_sub); - close(cmd_sub); - close(subsys_sub); - close(diff_pres_sub); - close(param_changed_sub); - close(battery_sub); - - warnx("exiting"); - fflush(stdout); - - thread_running = false; - - return 0; -} - - -void *commander_low_prio_loop(void *arg) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander low prio", getpid()); - - while (!thread_should_exit) { - - switch (low_prio_task) { - - case LOW_PRIO_TASK_PARAM_LOAD: - - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); - } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_PARAM_SAVE: - - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); - } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_GYRO_CALIBRATION: - - do_gyro_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_MAG_CALIBRATION: - - do_mag_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - - // do_baro_calibration(mavlink_fd); - - case LOW_PRIO_TASK_RC_CALIBRATION: - - // do_rc_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_ACCEL_CALIBRATION: - - do_accel_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - - do_airspeed_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; - break; - - case LOW_PRIO_TASK_NONE: - default: - /* slow down to 10Hz */ - usleep(100000); - break; - } - - } - - return 0; -} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp new file mode 100644 index 000000000..253b6295f --- /dev/null +++ b/src/modules/commander/commander.cpp @@ -0,0 +1,1720 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * 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 commander.cpp + * Main system state machine implementation. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "commander_helper.h" +#include "state_machine_helper.h" +#include "calibration_routines.h" +#include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +extern struct system_load_s system_load; + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + +/* Mavlink file descriptors */ +static int mavlink_fd; + +/* flags */ +static bool commander_initialized = false; +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* timout until lowlevel failsafe */ +static unsigned int failsafe_lowlevel_timeout_ms; + +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + * + * @ingroup apps + */ +extern "C" __EXPORT int commander_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +void usage(const char *reason); + +/** + * React to commands that are sent e.g. from the mavlink module. + */ +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); + + +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: + + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + /* reboot is executed later, after positive tune is sent */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); + } + } + } + break; + + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + + } else if (((int)(cmd->param1)) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + default: + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + + tune_negative(); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + + tune_positive(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + bool battery_tune_played = false; + bool arm_tune_played = false; + + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + + /* welcome user */ + warnx("[commander] starting"); + + /* pthread for slow low prio thread */ + pthread_t commander_low_prio_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds"); + } + + if (buzzer_init() != OK) { + warnx("ERROR: Failed to initialize buzzer"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + } + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; + + /* neither manual nor offboard control commands have been received */ + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; + + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; + + /* allow manual override initially */ + control_mode.flag_external_manual_override_ok = true; + + /* flag position info as bad, do not allow auto mode */ + // current_status.flag_vector_flight_mode_ok = false; + + /* set battery warning flag */ + current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + + /* set safety device detection flag */ + /* XXX do we need this? */ + //current_status.flag_safety_present = false; + + // XXX for now just set sensors as initialized + current_status.condition_system_sensors_initialized = true; + + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + + /* advertise to ORB */ + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + + + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + // XXX needed? + mavlink_log_info(mavlink_fd, "system is running"); + + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + + /* Start monitoring loop */ + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; + + /* To remember when last notification was sent */ + uint64_t last_print_time = 0; + + float voltage_previous = 0.0f; + + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + bool new_data = false; + + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + /* Subscribe to global position */ + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + /* Subscribe to local position data */ + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + + /* Subscribe to GPS topic */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + /* Subscribe to sensor topic */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + /* Subscribe to differential pressure topic */ + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + uint64_t last_diff_pres_time = 0; + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* Subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + start_time = hrt_absolute_time(); + + while (!thread_should_exit) { + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!armed.armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed getting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + control_mode.flag_external_manual_override_ok = false; + + } else { + control_mode.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + + orb_check(sp_man_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(diff_pres_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + last_diff_pres_time = diff_pres.timestamp; + } + + orb_check(cmd_sub, &new_data); + + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + } + + /* update safety topic */ + orb_check(safety_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(safety), safety_sub, &safety); + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; + } + + /* update subsystem */ + orb_check(subsys_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + current_status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + current_status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + current_status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_health &= ~info.subsystem_type; + } + } + + /* Slow but important 8 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + + /* XXX if armed */ + if (armed.armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ + led_toggle(LED_AMBER); + } + + if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { + + /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ + if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + /* GPS lock */ + led_on(LED_BLUE); + + } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* no GPS lock, but GPS module is aquiring lock */ + led_toggle(LED_BLUE); + } + + } else { + /* no GPS info, don't light the blue led */ + led_off(LED_BLUE); + } + + + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } + + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + + + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, change state machine */ + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + tune_critical_bat(); + // XXX implement state change here + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + // XXX check for sensors + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + } else { + // XXX: Add emergency stuff if sensors are lost + } + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_airspeed_valid = true; + + } else { + current_status.condition_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; + + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } + + /* consolidate state change, flag as changed if required */ + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it + + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } + + orb_check(gps_sub, &new_data); + if (new_data) { + + + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; + + /* check if gps fix is ok */ + // XXX magic number + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_positive(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* Start RC state check */ + + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + + warnx("mode sw not finite"); + /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + + } else { + + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if land/RTL is requested + */ + if (!isfinite(sp_man.return_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.return_switch = RETURN_SWITCH_RETURN; + + } else { + /* center stick position, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else { + + /* center switch position, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { + + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { + mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); + tune_negative(); + } else { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); + } + + } else { + mavlink_log_critical(mavlink_fd, "DISARM not allowed"); + tune_negative(); + } + stick_off_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position and we're in manual --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + stick_on_counter = 0; + tune_positive(); + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + state_changed = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } + } + } + + + + + /* End mode switch */ + + /* END RC state check */ + + + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; + + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } + + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } + + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); + + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + // XXX check if this is correct + /* arm / disarm on request */ + if (sp_offboard.armed && !armed.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + + } else if (!sp_offboard.armed && armed.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + } + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + tune_positive(); + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + current_status.failsave_lowlevel = true; + state_changed = true; + } + } + } + } + + + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + + + // XXX this is missing + /* If full run came back clean, transition to standby */ + // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + // current_status.flag_preflight_gyro_calibration == false && + // current_status.flag_preflight_mag_calibration == false && + // current_status.flag_preflight_accel_calibration == false) { + // /* All ok, no calibration going on, go to standby */ + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + state_changed = false; + } + + + + /* Store old modes to detect and act on state transitions */ + voltage_previous = current_status.battery_voltage; + + + /* play tone according to evaluation result */ + /* check if we recently armed */ + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (tune_arm() == OK) + arm_tune_played = true; + + /* Trigger audio event for low battery */ + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + if (tune_critical_bat() == OK) + battery_tune_played = true; + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + if (tune_low_bat() == OK) + battery_tune_played = true; + } else if(battery_tune_played) { + tune_stop(); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + + /* XXX use this voltage_previous */ + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + pthread_join(commander_low_prio_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(local_position_sub); + close(global_position_sub); + close(gps_sub); + close(sensor_sub); + close(safety_sub); + close(cmd_sub); + close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); + + warnx("exiting"); + fflush(stdout); + + thread_running = false; + + return 0; +} + + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + + // do_baro_calibration(mavlink_fd); + + case LOW_PRIO_TASK_RC_CALIBRATION: + + // do_rc_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h deleted file mode 100644 index 6e57c0ba5..000000000 --- a/src/modules/commander/commander.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes - * - * 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 commander.h - * Main system state machine definition. - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - */ - -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - - - -#endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c deleted file mode 100644 index 199f73e6c..000000000 --- a/src/modules/commander/commander_helper.c +++ /dev/null @@ -1,218 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * 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 commander_helper.c - * Commander helper functions implementations - */ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "commander_helper.h" - -bool is_multirotor(const struct vehicle_status_s *current_status) -{ - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); -} - -bool is_rotary_wing(const struct vehicle_status_s *current_status) -{ - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); -} - -static int buzzer; - -int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return OK; -} - -void buzzer_deinit() -{ - close(buzzer); -} - -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, 2); -} - -void tune_positive() -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_neutral() -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void tune_negative() -{ - ioctl(buzzer, TONE_SET_ALARM, 5); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, 12); -} - -int tune_critical_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, 14); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, 13); -} - -void tune_stop() -{ - ioctl(buzzer, TONE_SET_ALARM, 0); -} - -static int leds; - -int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -void led_deinit() -{ - close(leds); -} - -int led_toggle(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); -} - -int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} \ No newline at end of file diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp new file mode 100644 index 000000000..9427bd892 --- /dev/null +++ b/src/modules/commander/commander_helper.cpp @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 commander_helper.cpp + * Commander helper functions implementations + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, 12); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 14); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, 0); +} + +static int leds; + +int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +void led_deinit() +{ + close(leds); +} + +int led_toggle(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); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; +} \ No newline at end of file diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c new file mode 100644 index 000000000..6832fc5ce --- /dev/null +++ b/src/modules/commander/commander_params.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 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 commander_params.c + * + * Parameters defined by the sensors task. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + */ + +#include +#include + +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c deleted file mode 100644 index 865f74ab4..000000000 --- a/src/modules/commander/gyro_calibration.c +++ /dev/null @@ -1,231 +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 gyro_calibration.c - * Gyroscope calibration routine - */ - -#include "gyro_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -void do_gyro_calibration(int mavlink_fd) -{ - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - - - - /*** --- SCALING --- ***/ - - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); - mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); - warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); - - unsigned rotations_count = 30; - float gyro_integral = 0.0f; - float baseline_integral = 0.0f; - - // XXX change to mag topic - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; - if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; - - - uint64_t last_time = hrt_absolute_time(); - uint64_t start_time = hrt_absolute_time(); - - while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { - - /* abort this loop if not rotated more than 180 degrees within 5 seconds */ - if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) - break; - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - - float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; - last_time = hrt_absolute_time(); - - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - - // XXX this is just a proof of concept and needs world / body - // transformation and more - - //math::Vector2f magNav(raw.magnetometer_ga); - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2*M_PI_F; - if (mag < -M_PI_F) mag += 2*M_PI_F; - - float diff = mag - mag_last; - - if (diff > M_PI_F) diff -= 2*M_PI_F; - if (diff < -M_PI_F) diff += 2*M_PI_F; - - baseline_integral += diff; - mag_last = mag; - // Jump through some timing scale hoops to avoid - // operating near the 1e6/1e8 max sane resolution of float. - gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - - warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); - - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; - } - } - - float gyro_scale = baseline_integral / gyro_integral; - warnx("gyro scale: yaw (z): %6.4f", gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); - - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp new file mode 100644 index 000000000..9e6909db0 --- /dev/null +++ b/src/modules/commander/gyro_calibration.cpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * 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 gyro_calibration.cpp + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + // XXX negative tune + return; + } + + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + return; + } + + + /*** --- SCALING --- ***/ + + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; + if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) + break; + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag > M_PI_F) mag -= 2*M_PI_F; + if (mag < -M_PI_F) mag += 2*M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2*M_PI_F; + if (diff < -M_PI_F) diff += 2*M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + + warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; + warnx("gyro scale: yaw (z): %6.4f", gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + + + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { + + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + gyro_scales[0], + gyro_offset[1], + gyro_scales[1], + gyro_offset[2], + gyro_scales[2], + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c deleted file mode 100644 index dbd31a7e7..000000000 --- a/src/modules/commander/mag_calibration.c +++ /dev/null @@ -1,278 +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 mag_calibration.c - * Magnetometer calibration routine - */ - -#include "mag_calibration.h" -#include "commander_helper.h" -#include "calibration_routines.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -void do_mag_calibration(int mavlink_fd) -{ - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_neutral(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - close(sub_mag); -} \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp new file mode 100644 index 000000000..9a25103f8 --- /dev/null +++ b/src/modules/commander/mag_calibration.cpp @@ -0,0 +1,280 @@ +/**************************************************************************** + * + * 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 mag_calibration.cpp + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fef8e366b..91d75121e 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,14 +36,15 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - commander_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c \ - gyro_calibration.c \ - mag_calibration.c \ - baro_calibration.c \ - rc_calibration.c \ - airspeed_calibration.c +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c deleted file mode 100644 index a21d3f558..000000000 --- a/src/modules/commander/rc_calibration.c +++ /dev/null @@ -1,83 +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 rc_calibration.c - * Remote Control calibration routine - */ - -#include "rc_calibration.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include - - -void do_rc_calibration(int mavlink_fd) -{ - mavlink_log_info(mavlink_fd, "trim calibration starting"); - - /* XXX fix this */ - // if (current_status.rc_signal) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - tune_positive(); - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} \ No newline at end of file diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp new file mode 100644 index 000000000..0de411713 --- /dev/null +++ b/src/modules/commander/rc_calibration.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 rc_calibration.cpp + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include + + +void do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.rc_signal) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c deleted file mode 100644 index 792ead8f3..000000000 --- a/src/modules/commander/state_machine_helper.c +++ /dev/null @@ -1,758 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * - * 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 state_machine_helper.c - * State machine helper functions implementations - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "state_machine_helper.h" -#include "commander_helper.h" - - -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; - } else { - - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED) { - - /* sensors need to be initialized for STANDBY state */ - if (current_state->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: - - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { - - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - - } - break; - case ARMING_STATE_IN_AIR_RESTORE: - - /* XXX implement */ - break; - default: - break; - } - - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); - } - } - - return ret; -} - - - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; - - /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; - } else { - - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: - - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { - - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_MANUAL_STANDBY: - - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_MANUAL: - - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; - - case NAVIGATION_STATE_SEATBELT_STANDBY: - - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { - - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_SEATBELT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_SEATBELT_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - default: - break; - } - - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); - } - } - - - - return ret; -} - - -/** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case HIL_STATE_OFF: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; - - case HIL_STATE_ON: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; - - default: - warnx("Unknown hil state"); - break; - } - } - - if (valid_transition) { - current_status->hil_state = new_state; - - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - - current_control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); - - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); - } - - return ret; -} - - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} - diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp new file mode 100644 index 000000000..3cf60a99a --- /dev/null +++ b/src/modules/commander/state_machine_helper.cpp @@ -0,0 +1,763 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 state_machine_helper.cpp + * State machine helper functions implementations + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "state_machine_helper.h" +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { + + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_state->arming_state == ARMING_STATE_STANDBY) { + ret = OK; + armed->armed = false; + armed->ready_to_arm = false; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->condition_system_sensors_initialized) { + ret = OK; + armed->armed = false; + armed->ready_to_arm = true; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ + ret = OK; + armed->armed = true; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_ARMED) { + + /* XXX conditions for an error state? */ + ret = OK; + armed->armed = true; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + armed->armed = false; + armed->ready_to_arm = false; + } + break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + armed->armed = false; + armed->ready_to_arm = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + default: + break; + } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + armed->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + } + } + + return ret; +} + + + +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: + + /* transitions back to INIT are possible for calibration */ + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + + ret = OK; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; + } + } + break; + + case NAVIGATION_STATE_MANUAL: + + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; + } + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); + } else if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state-> condition_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + default: + break; + } + + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + } + } + + + + return ret; +} + + +/** +* Transition from one hil state to another +*/ +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} + diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e52..356215b0e 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,12 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +//extern void up_systemreset(void) noreturn_function; +#include <../arch/common/up_internal.h> + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); -- cgit v1.2.3 From eb52eae153bc83144ddb5d6d03fdbb22aa0c9203 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:48:19 +0200 Subject: Code style for BlinkM --- src/drivers/blinkm/blinkm.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 9bb020a6b..aa94be493 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -866,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --blinkmaddr blinkmaddr (9)"); } int -- cgit v1.2.3 From cfbdf7c9037c948e869fc805a79fb92e5441e15a Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Jul 2013 18:51:25 -0700 Subject: Revert "Corrected the interval of the MS5611" This reverts commit 094ff1261aa4a651e898c50d4451d283cb899a72. --- src/drivers/ms5611/ms5611.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 122cf0cec..452416035 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -336,9 +336,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); - /* set interval to minimum legal value */ - _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; - + /* set interval for next measurement to minimum legal value */ + _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) @@ -352,12 +351,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* do we need to start internal polling? */ bool want_start = (_call_baro_interval == 0); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + /* check against maximum rate */ - if (1000000/arg < MS5611_CONVERSION_INTERVAL) + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) return -EINVAL; - /* update interval */ - _baro_call.period = _call_baro_interval = 1000000/arg; + /* update interval for next measurement */ + _baro_call.period = _call_baro_interval = ticks; /* if we need to start the poll state machine, do it */ if (want_start) -- cgit v1.2.3 From f6daafba03fab76bac41fbaac60ea8c4d324c65b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Jul 2013 18:51:46 -0700 Subject: Revert "Changed the MS5611 from the workq to hrt_call_every implementation, this seems to solve the SPI chip select overlaps" This reverts commit c93e743374731ec06020ba6919c6d48594d4a58c. --- src/drivers/ms5611/ms5611.cpp | 138 +++++++++++++++++++++++++----------------- 1 file changed, 84 insertions(+), 54 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 452416035..d9268c0b3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,9 +111,8 @@ protected: ms5611::prom_s _prom; - struct hrt_call _baro_call; - - unsigned _call_baro_interval; + struct work_s _work; + unsigned _measure_ticks; unsigned _num_reports; volatile unsigned _next_report; @@ -144,12 +143,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start_cycle(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop_cycle(); /** * Perform a poll cycle; collect from the previous measurement @@ -167,11 +166,12 @@ protected: void cycle(); /** - * Static trampoline; XXX is this needed? + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,7 +184,6 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); - }; /* @@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _call_baro_interval(0), + _measure_ticks(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop(); + stop_cycle(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_call_baro_interval > 0) { + if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - _measure_phase = 0; - _oldest_report = _next_report = 0; - measure(); + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + } while (0); return ret; } @@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); - _call_baro_interval = 0; + stop_cycle(); + _measure_ticks = 0; return OK; /* external signalling not supported */ @@ -334,14 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); + bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _baro_call.period = _call_baro_interval = USEC2TICK(MS5611_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -349,7 +377,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); + bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); @@ -359,11 +387,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* update interval for next measurement */ - _baro_call.period = _call_baro_interval = ticks; + _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -371,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_baro_interval == 0) + if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _call_baro_interval); + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -391,11 +419,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop(); + stop_cycle(); delete[] _reports; _num_reports = arg; _reports = buf; - start(); + start_cycle(); return OK; } @@ -429,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start() +MS5611::start_cycle() { - /* make sure we are stopped first */ - stop(); - /* reset the report ring */ - _oldest_report = _next_report = 0; - /* reset to first measure phase */ + /* reset the report ring and state machine */ + _collect_phase = false; _measure_phase = 0; + _oldest_report = _next_report = 0; /* schedule a cycle to start things */ - hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); } void -MS5611::stop() +MS5611::stop_cycle() { - hrt_cancel(&_baro_call); + work_cancel(HPWORK, &_work); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611 *dev = reinterpret_cast(arg); - cycle(); + dev->cycle(); } void @@ -478,7 +504,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -491,16 +517,14 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - // XXX maybe do something appropriate as well + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { -// /* schedule a fresh cycle call when we are ready to measure again */ -// work_queue(HPWORK, -// &_work, -// (worker_t)&MS5611::cycle_trampoline, -// this, -// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -511,12 +535,19 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } /* next phase is collection */ _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -665,14 +696,13 @@ MS5611::collect() return OK; } - void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _call_baro_interval); + printf("poll interval: %u ticks\n", _measure_ticks); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); -- cgit v1.2.3 From 31ded04c80f68bd071840770d49eb93d2d9c9fe2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 20 Jul 2013 19:09:59 -0700 Subject: Disable interrupts during ms5611 SPI transactions to prevent interruption by sensor drivers polling other sensors from interrupt context. --- src/drivers/ms5611/ms5611.cpp | 148 +++++++++++++++----------------------- src/drivers/ms5611/ms5611_spi.cpp | 28 ++++++-- 2 files changed, 82 insertions(+), 94 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b3..122cf0cec 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,8 +111,9 @@ protected: ms5611::prom_s _prom; - struct work_s _work; - unsigned _measure_ticks; + struct hrt_call _baro_call; + + unsigned _call_baro_interval; unsigned _num_reports; volatile unsigned _next_report; @@ -143,12 +144,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -166,12 +167,11 @@ protected: void cycle(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. + * Static trampoline; XXX is this needed? * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,6 +184,7 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); + }; /* @@ -195,7 +196,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _measure_ticks(0), + _call_baro_interval(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -212,14 +213,13 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_call_baro_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,41 +298,13 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + _measure_phase = 0; + _oldest_report = _next_report = 0; + measure(); - } while (0); + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -347,8 +319,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; + stop(); + _call_baro_interval = 0; return OK; /* external signalling not supported */ @@ -362,14 +334,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); + + /* set interval to minimum legal value */ + _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -377,21 +350,18 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + bool want_start = (_call_baro_interval == 0); /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if (1000000/arg < MS5611_CONVERSION_INTERVAL) return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval */ + _baro_call.period = _call_baro_interval = 1000000/arg; /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -399,10 +369,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_call_baro_interval == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return (1000 / _call_baro_interval); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -419,11 +389,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); + stop(); delete[] _reports; _num_reports = arg; _reports = buf; - start_cycle(); + start(); return OK; } @@ -457,30 +427,32 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start() { + /* make sure we are stopped first */ + stop(); - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; + /* reset the report ring */ _oldest_report = _next_report = 0; + /* reset to first measure phase */ + _measure_phase = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); } void -MS5611::stop_cycle() +MS5611::stop() { - work_cancel(HPWORK, &_work); + hrt_cancel(&_baro_call); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = reinterpret_cast(arg); + MS5611 *dev = (MS5611 *)arg; - dev->cycle(); + cycle(); } void @@ -504,7 +476,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } @@ -517,14 +489,16 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + // XXX maybe do something appropriate as well - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); +// /* schedule a fresh cycle call when we are ready to measure again */ +// work_queue(HPWORK, +// &_work, +// (worker_t)&MS5611::cycle_trampoline, +// this, +// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -535,19 +509,12 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } /* next phase is collection */ _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -696,13 +663,14 @@ MS5611::collect() return OK; } + void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u ticks\n", _call_baro_interval); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 156832a61..1ea698922 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -103,6 +103,14 @@ private: * @param reg The register to read. */ uint16_t _reg16(unsigned reg); + + /** + * Wrapper around transfer() that prevents interrupt-context transfers + * from pre-empting us. The sensor may (does) share a bus with sensors + * that are polled from interrupt context (or we may be pre-empted) + * so we need to guarantee that transfers complete without interruption. + */ + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; device::Device * @@ -161,7 +169,7 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ buf[0] = 0 | DIR_WRITE; - int ret = transfer(&buf[0], &buf[0], sizeof(buf)); + int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { /* fetch the raw value */ @@ -206,7 +214,7 @@ MS5611_SPI::_reset() { uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } int @@ -214,7 +222,7 @@ MS5611_SPI::_measure(unsigned addr) { uint8_t cmd = addr | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } @@ -244,9 +252,21 @@ MS5611_SPI::_reg16(unsigned reg) cmd[0] = reg | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); + _transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } +int +MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + irqstate_t flags = irqsave(); + + int ret = transfer(send, recv, len); + + irqrestore(flags); + + return ret; +} + #endif /* PX4_SPIDEV_BARO */ -- cgit v1.2.3 From 98a4345410e91a1e3966b3364f487652af210d03 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 21 Jul 2013 22:42:45 +0400 Subject: multirotor_pos_control: some refactoring and cleanup, attitude-thrust correction moved to thrust_pid --- .../multirotor_pos_control.c | 39 ++++++-------- .../multirotor_pos_control_params.c | 6 +-- .../multirotor_pos_control_params.h | 4 +- src/modules/multirotor_pos_control/thrust_pid.c | 61 ++++++++++++---------- src/modules/multirotor_pos_control/thrust_pid.h | 2 +- 5 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b49186ee9..6252178d1 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); @@ -259,7 +259,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); @@ -416,12 +416,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* run position & altitude controllers, calculate velocity setpoint */ - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; @@ -439,9 +439,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - /* run velocity controllers, calculate thrust vector */ + /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ @@ -452,33 +452,24 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + /* assuming that vertical component of thrust is g, + * horizontal component = g * tan(alpha) */ + float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - if (thrust_xy_norm > params.slope_max) { - thrust_xy_norm = params.slope_max; + if (tilt > params.tilt_max) { + tilt = params.tilt_max; } - /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; - att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + att_sp.roll_body = sinf(thrust_xy_dir) * tilt; + att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } - /* attitude-thrust compensation */ - float att_comp; - - if (att.R[2][2] > 0.8f) - att_comp = 1.0f / att.R[2][2]; - else if (att.R[2][2] > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; - else - att_comp = 1.0f; - - att_sp.thrust = -thrust_sp[2] * att_comp; + att_sp.thrust = -thrust_sp[2]; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index f8a982c6c..1094fd23b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -74,7 +74,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->xy_vel_i = param_find("MPC_XY_VEL_I"); h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->slope_max = param_find("MPC_SLOPE_MAX"); + h->tilt_max = param_find("MPC_TILT_MAX"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -99,7 +99,7 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->xy_vel_i, &(p->xy_vel_i)); param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->slope_max, &(p->slope_max)); + param_get(h->tilt_max, &(p->tilt_max)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index e9b1f5c39..14a3de2e5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -56,7 +56,7 @@ struct multirotor_position_control_params { float xy_vel_i; float xy_vel_d; float xy_vel_max; - float slope_max; + float tilt_max; float rc_scale_pitch; float rc_scale_roll; @@ -78,7 +78,7 @@ struct multirotor_position_control_param_handles { param_t xy_vel_i; param_t xy_vel_d; param_t xy_vel_max; - param_t slope_max; + param_t tilt_max; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 89efe1334..51dcd7df2 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -1,11 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,13 +35,9 @@ /** * @file thrust_pid.c * - * Implementation of generic PID control interface. + * Implementation of thrust control PID. * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann * @author Anton Babushkin - * @author Julian Oes */ #include "thrust_pid.h" @@ -108,16 +100,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl return ret; } -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) { /* Alternative integral component calculation - error = setpoint - actual_position - integral = integral + (Ki*error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + integral + (Kd*derivative) - previous_error = error - wait(dt) - goto start + * + * start: + * error = setpoint - current_value + * integral = integral + (Ki * error * dt) + * derivative = (error - previous_error) / dt + * previous_error = error + * output = (Kp * error) + integral + (Kd * derivative) + * wait(dt) + * goto start */ if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { @@ -147,21 +141,32 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa d = 0.0f; } - // Calculate the error integral and check for saturation + /* calculate the error integral */ i = pid->integral + (pid->ki * error * dt); - float output = (error * pid->kp) + i + (d * pid->kd); + /* attitude-thrust compensation + * r22 is (2, 2) componet of rotation matrix for current attitude */ + float att_comp; + + if (r22 > 0.8f) + att_comp = 1.0f / r22; + else if (r22 > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; + else + att_comp = 1.0f; + + /* calculate output */ + float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + + /* check for saturation */ if (output < pid->limit_min || output > pid->limit_max) { - i = pid->integral; // If saturated then do not update integral value - // recalculate output with old integral - output = (error * pid->kp) + i + (d * pid->kd); + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); } else { - if (!isfinite(i)) { - i = 0.0f; + if (isfinite(i)) { + pid->integral = i; } - - pid->integral = i; } if (isfinite(output)) { diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 65ee33c51..551c032fe 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -68,7 +68,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); __END_DECLS -- cgit v1.2.3 From bd50092dd29a83dd5044fc3027e20be91b3275e5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 21 Jul 2013 22:44:46 +0400 Subject: position_estimator_inav: accelerometer bias estimation for Z, default weights for GPS updated --- .../position_estimator_inav_main.c | 20 ++++++++++++++------ .../position_estimator_inav_params.c | 9 ++++++--- .../position_estimator_inav_params.h | 2 ++ 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a9bc09e0d..42b5fcb61 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,9 +167,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0; //[°]] --> 47.0 - double lon_current = 0.0; //[°]] -->8.5 - double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0f; //[°] --> 47.0 + double lon_current = 0.0f; //[°] --> 8.5 + double alt_current = 0.0f; //[m] above MSL uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -302,8 +302,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float baro_corr = 0.0f; // D float gps_corr[2][2] = { { 0.0f, 0.0f }, // N (pos, vel) @@ -369,9 +373,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { + /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[2] -= accel_bias[2]; /* transform acceleration vector from body frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -425,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.home_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; - sonar_corr_filtered = 0.0; + sonar_corr_filtered = 0.0f; } } } @@ -470,6 +474,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* accel bias correction, now only for Z + * not strictly correct, but stable and works */ + accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index c90c611a7..70248b3b7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -44,10 +44,11 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); @@ -62,6 +63,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); @@ -79,6 +81,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index cca172b5d..1e70a3c48 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -49,6 +49,7 @@ struct position_estimator_inav_params { float w_pos_gps_v; float w_pos_acc; float w_pos_flow; + float w_acc_bias; float flow_k; float sonar_filt; float sonar_err; @@ -63,6 +64,7 @@ struct position_estimator_inav_param_handles { param_t w_pos_gps_v; param_t w_pos_acc; param_t w_pos_flow; + param_t w_acc_bias; param_t flow_k; param_t sonar_filt; param_t sonar_err; -- cgit v1.2.3 From 963abd66badf71a925f80e12312c429d64999424 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 21:48:21 +0400 Subject: commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing --- src/modules/commander/commander.cpp | 190 ++++++++++++---------- src/modules/commander/state_machine_helper.cpp | 66 ++++++-- src/modules/mavlink/mavlink.c | 12 +- src/modules/uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/vehicle_status.h | 17 +- 5 files changed, 178 insertions(+), 108 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 253b6295f..25c5a84ea 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { current_status.condition_global_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { current_status.condition_local_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_local_position_valid = false; @@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_SEATBELT; + current_status.mode_switch = MODE_SWITCH_ASSISTED; } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[]) /* this switch is not properly mapped, set default */ current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ current_status.return_switch = RETURN_SWITCH_RETURN; } else { - /* center stick position, set default */ + /* center or bottom stick position, set default */ current_status.return_switch = RETURN_SWITCH_NONE; } + /* check assisted switch */ + if (!isfinite(sp_man.assisted_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; + + } else { + /* center or bottom stick position, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { @@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } - /* Try seatbelt or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + /* Try assisted or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the navigation state when armed */ case ARMING_STATE_ARMED: - /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - + /* MANUAL */ if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } - /* SEATBELT_STANDBY (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_NONE) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* SEATBELT_DESCENT (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_RETURN) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_NONE) { - - /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { + /* ASSISTED */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* ASSISTED_DESCENT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); } } - /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + + } else { + if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { + /* ASSISTED_SIMPLE */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* ASSISTED_SEATBELT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } } - /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_MISSION) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + /* AUTO */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* AUTO_RTL */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_DESCENT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } - } - - /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_RETURN - && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else { + if (current_status.mission_switch == MISSION_SWITCH_MISSION) { + /* AUTO_MISSION */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + // TODO check this + if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* AUTO_LOITER */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cf60a99a..60ab01536 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions back to INIT are possible for calibration */ if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; @@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { @@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_STANDBY: + case NAVIGATION_STATE_ASSISTED_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { @@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT: + case NAVIGATION_STATE_ASSISTED_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_DESCENT: + case NAVIGATION_STATE_ASSISTED_SIMPLE: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_ASSISTED_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT or from other STANDBY modes or from AUTO READY */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { /* need to be disarmed and have a position and home lock */ @@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ @@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ @@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index db8ee9067..ae8e168e1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,9 +221,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index cfee81ea2..eac6d6e98 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -58,6 +58,7 @@ struct manual_control_setpoint_s { float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ec08a6c13..9b91e834e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -60,12 +60,13 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_INIT = 0, NAVIGATION_STATE_MANUAL_STANDBY, NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT_STANDBY, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_ASSISTED_STANDBY, + NAVIGATION_STATE_ASSISTED_SEATBELT, + NAVIGATION_STATE_ASSISTED_SIMPLE, + NAVIGATION_STATE_ASSISTED_DESCENT, NAVIGATION_STATE_AUTO_STANDBY, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_AUTO_TAKEOFF, @@ -98,7 +99,7 @@ typedef enum { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_SEATBELT, + MODE_SWITCH_ASSISTED, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -107,6 +108,11 @@ typedef enum { RETURN_SWITCH_RETURN } return_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_SIMPLE +} assisted_switch_pos_t; + typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -179,6 +185,7 @@ struct vehicle_status_s mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; + assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; bool condition_battery_voltage_valid; -- cgit v1.2.3 From 55fd19f2813110d14d536943503851255c997b6f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 22:37:10 +0400 Subject: sensors: ASSISTED switch channel added --- src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++++++++++++- src/modules/uORB/topics/rc_channels.h | 24 ++++++++++++++---------- 3 files changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3ed9efc8b..d0af9e17b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,6 +167,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 89d5cd374..b38dc8d89 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -208,6 +208,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; + int rc_map_assisted_sw; int rc_map_mission_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -256,6 +257,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; + param_t rc_map_assisted_sw; param_t rc_map_mission_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -464,6 +466,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -617,6 +620,10 @@ Sensors::parameters_update() warnx("Failed getting return sw chan index"); } + if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + warnx("Failed getting assisted sw chan index"); + } + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { warnx("Failed getting mission sw chan index"); } @@ -673,6 +680,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1142,6 +1150,7 @@ Sensors::ppm_poll() manual_control.mode_switch = NAN; manual_control.return_switch = NAN; + manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; @@ -1249,7 +1258,10 @@ Sensors::ppm_poll() /* land switch input */ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - /* land switch input */ + /* assisted switch input */ + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + + /* mission switch input */ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index a0bb25af4..2167e44a2 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -53,9 +53,12 @@ /** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, - * leaving at a sane value of 14. + * leaving at a sane value of 15. + * This number can be greater then number of RC channels, + * because single RC channel can be mapped to multiple + * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 14 +#define RC_CHANNELS_MAX 15 /** * This defines the mapping of the RC functions. @@ -70,14 +73,15 @@ enum RC_CHANNELS_FUNCTION YAW = 3, MODE = 4, RETURN = 5, - MISSION = 6, - OFFBOARD_MODE = 7, - FLAPS = 8, - AUX_1 = 9, - AUX_2 = 10, - AUX_3 = 11, - AUX_4 = 12, - AUX_5 = 13, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; -- cgit v1.2.3 From 7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 23 Jul 2013 14:56:12 +0400 Subject: commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed --- src/modules/commander/commander.cpp | 39 +++++++++----------------- src/modules/commander/state_machine_helper.cpp | 24 ++++++++++++---- src/modules/uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25c5a84ea..1978d8782 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - + /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + * TODO allow disarm when landed + */ + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && + control_mode.flag_control_manual_enabled && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); stick_off_counter = 0; } else { @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower right position and we're in manual mode --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 60ab01536..4a7aa66b7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = false; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = false; } break; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } } @@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } break; @@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } break; @@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 8481a624f..d83eb45d9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ -- cgit v1.2.3 From 8dd5130d998f7609c8a5d457093e31320b3668fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 24 Jul 2013 18:20:04 +0400 Subject: position_estimator_inav, multirotor_pos_control: bugs fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 3 ++- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6252178d1..10f21c55d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -259,7 +259,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); + // TODO 1000.0 is hotfix + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 42b5fcb61..d4b2f0e43 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -453,8 +453,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) gps_corr[1][0] = gps_proj[1] - y_est[0]; if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; } else { gps_corr[0][1] = 0.0f; -- cgit v1.2.3 From d57d12818a3c79cc992ff70e765734e7145603d0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 24 Jul 2013 22:51:27 -0700 Subject: Revert "Disable interrupts during ms5611 SPI transactions to prevent interruption by sensor drivers polling other sensors from interrupt context." This reverts commit 31ded04c80f68bd071840770d49eb93d2d9c9fe2. --- src/drivers/ms5611/ms5611.cpp | 148 +++++++++++++++++++++++--------------- src/drivers/ms5611/ms5611_spi.cpp | 28 ++------ 2 files changed, 94 insertions(+), 82 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 122cf0cec..d9268c0b3 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,9 +111,8 @@ protected: ms5611::prom_s _prom; - struct hrt_call _baro_call; - - unsigned _call_baro_interval; + struct work_s _work; + unsigned _measure_ticks; unsigned _num_reports; volatile unsigned _next_report; @@ -144,12 +143,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start_cycle(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop_cycle(); /** * Perform a poll cycle; collect from the previous measurement @@ -167,11 +166,12 @@ protected: void cycle(); /** - * Static trampoline; XXX is this needed? + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,7 +184,6 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); - }; /* @@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _call_baro_interval(0), + _measure_ticks(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop(); + stop_cycle(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_call_baro_interval > 0) { + if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - _measure_phase = 0; - _oldest_report = _next_report = 0; - measure(); + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); return ret; } @@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop(); - _call_baro_interval = 0; + stop_cycle(); + _measure_ticks = 0; return OK; /* external signalling not supported */ @@ -334,15 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); - - /* set interval to minimum legal value */ - _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; + bool want_start = (_measure_ticks == 0); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -350,18 +377,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_call_baro_interval == 0); + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (1000000/arg < MS5611_CONVERSION_INTERVAL) + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) return -EINVAL; - /* update interval */ - _baro_call.period = _call_baro_interval = 1000000/arg; + /* update interval for next measurement */ + _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ if (want_start) - start(); + start_cycle(); return OK; } @@ -369,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_baro_interval == 0) + if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _call_baro_interval); + return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -389,11 +419,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop(); + stop_cycle(); delete[] _reports; _num_reports = arg; _reports = buf; - start(); + start_cycle(); return OK; } @@ -427,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start() +MS5611::start_cycle() { - /* make sure we are stopped first */ - stop(); - /* reset the report ring */ - _oldest_report = _next_report = 0; - /* reset to first measure phase */ + /* reset the report ring and state machine */ + _collect_phase = false; _measure_phase = 0; + _oldest_report = _next_report = 0; /* schedule a cycle to start things */ - hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); } void -MS5611::stop() +MS5611::stop_cycle() { - hrt_cancel(&_baro_call); + work_cancel(HPWORK, &_work); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611 *dev = reinterpret_cast(arg); - cycle(); + dev->cycle(); } void @@ -476,7 +504,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } @@ -489,16 +517,14 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { - - // XXX maybe do something appropriate as well + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { -// /* schedule a fresh cycle call when we are ready to measure again */ -// work_queue(HPWORK, -// &_work, -// (worker_t)&MS5611::cycle_trampoline, -// this, -// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -509,12 +535,19 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start(); + start_cycle(); return; } /* next phase is collection */ _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -663,14 +696,13 @@ MS5611::collect() return OK; } - void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _call_baro_interval); + printf("poll interval: %u ticks\n", _measure_ticks); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 1ea698922..156832a61 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -103,14 +103,6 @@ private: * @param reg The register to read. */ uint16_t _reg16(unsigned reg); - - /** - * Wrapper around transfer() that prevents interrupt-context transfers - * from pre-empting us. The sensor may (does) share a bus with sensors - * that are polled from interrupt context (or we may be pre-empted) - * so we need to guarantee that transfers complete without interruption. - */ - int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; device::Device * @@ -169,7 +161,7 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ buf[0] = 0 | DIR_WRITE; - int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); + int ret = transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { /* fetch the raw value */ @@ -214,7 +206,7 @@ MS5611_SPI::_reset() { uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - return _transfer(&cmd, nullptr, 1); + return transfer(&cmd, nullptr, 1); } int @@ -222,7 +214,7 @@ MS5611_SPI::_measure(unsigned addr) { uint8_t cmd = addr | DIR_WRITE; - return _transfer(&cmd, nullptr, 1); + return transfer(&cmd, nullptr, 1); } @@ -252,21 +244,9 @@ MS5611_SPI::_reg16(unsigned reg) cmd[0] = reg | DIR_READ; - _transfer(cmd, cmd, sizeof(cmd)); + transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } -int -MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) -{ - irqstate_t flags = irqsave(); - - int ret = transfer(send, recv, len); - - irqrestore(flags); - - return ret; -} - #endif /* PX4_SPIDEV_BARO */ -- cgit v1.2.3 From 33e33d71b8177fd3c1c9972e13d14184a5428e42 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 24 Jul 2013 22:53:10 -0700 Subject: Just the changes required to avoid SPI bus conflicts for MS5611 --- src/drivers/ms5611/ms5611_spi.cpp | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 156832a61..1ea698922 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -103,6 +103,14 @@ private: * @param reg The register to read. */ uint16_t _reg16(unsigned reg); + + /** + * Wrapper around transfer() that prevents interrupt-context transfers + * from pre-empting us. The sensor may (does) share a bus with sensors + * that are polled from interrupt context (or we may be pre-empted) + * so we need to guarantee that transfers complete without interruption. + */ + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; device::Device * @@ -161,7 +169,7 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ buf[0] = 0 | DIR_WRITE; - int ret = transfer(&buf[0], &buf[0], sizeof(buf)); + int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { /* fetch the raw value */ @@ -206,7 +214,7 @@ MS5611_SPI::_reset() { uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } int @@ -214,7 +222,7 @@ MS5611_SPI::_measure(unsigned addr) { uint8_t cmd = addr | DIR_WRITE; - return transfer(&cmd, nullptr, 1); + return _transfer(&cmd, nullptr, 1); } @@ -244,9 +252,21 @@ MS5611_SPI::_reg16(unsigned reg) cmd[0] = reg | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); + _transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; } +int +MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + irqstate_t flags = irqsave(); + + int ret = transfer(send, recv, len); + + irqrestore(flags); + + return ret; +} + #endif /* PX4_SPIDEV_BARO */ -- cgit v1.2.3 From ffd14e1396fd216651c44615b06605f9e9f975e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jul 2013 12:44:47 +0200 Subject: Updated F330 script, enabled amber led --- ROMFS/px4fmu_common/init.d/10_io_f330 | 15 +++++++-------- src/drivers/boards/px4fmuv2/px4fmu2_init.c | 4 ++-- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 4 ++-- 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 4107fab4f..07e70993d 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -15,20 +15,19 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.007 + param set MC_ATTRATE_D 0.005 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_P 0.1 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_POS_P 0.1 + param set MC_ATT_P 4.5 param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.5 - param set MC_YAWPOS_P 1.0 + param set MC_YAWPOS_I 0.3 + param set MC_YAWPOS_P 0.6 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.1 param save /fs/microsd/params fi @@ -128,7 +127,7 @@ multirotor_att_control start # # Start logging # -sdlog2 start -r 20 -a -b 14 +sdlog2 start -r 20 -a -b 16 # # Start system state diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c index 535e075b2..13829d5c4 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_init.c @@ -193,8 +193,8 @@ __EXPORT int nsh_archinitialize(void) NULL); /* initial LED state */ - //drv_led_start(); - up_ledoff(LED_AMBER); + drv_led_start(); + led_off(LED_AMBER); /* Configure SPI-based devices */ diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c index 85177df56..11a5c7211 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -68,7 +68,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 0) + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); @@ -77,7 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 0) + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); -- cgit v1.2.3 From a013fc5d0b28cd72f41a28c8229c2d7ab99965f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 25 Jul 2013 20:05:45 +0400 Subject: multirotor_pos_control: limit xy velocity integral output to tilt_max / 2 --- .../multirotor_pos_control/multirotor_pos_control.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 10f21c55d..44d478a9e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -248,9 +248,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* check parameters at 1 Hz*/ - paramcheck_counter++; - - if (paramcheck_counter == 50) { + if (--paramcheck_counter <= 0) { + paramcheck_counter = 50; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -259,15 +258,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - // TODO 1000.0 is hotfix - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; + if (params.xy_vel_i == 0.0) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { + i_limit = 1.0f; // not used really + } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } - - paramcheck_counter = 0; } /* only check global position setpoint updates but not read */ -- cgit v1.2.3 From dd3033fa6fa8ea4b84582065ed9c92828d7c5f81 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Jul 2013 15:16:48 +0200 Subject: Symbol cleanup for servo vs. battery voltage --- src/drivers/px4io/px4io.cpp | 6 +++--- src/modules/px4iofirmware/protocol.h | 12 ++++++------ src/modules/px4iofirmware/registers.c | 26 +++++++++----------------- 3 files changed, 18 insertions(+), 26 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b019c4fc5..dd876f903 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1315,7 +1315,7 @@ PX4IO::print_status() io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); if (_hardware == 1) { - printf("vbatt %u ibatt %u vbatt scale %u\n", + printf("vbatt mV %u ibatt mV %u vbatt scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); @@ -1324,9 +1324,9 @@ PX4IO::print_status() (double)_battery_amp_bias, (double)_battery_mamphour_total); } else if (_hardware == 2) { - printf("vservo %u vservo scale %u\n", + printf("vservo %u mV vservo scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE)); printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); } printf("actuators"); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index dc5c40638..3c59a75a7 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -163,13 +163,13 @@ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* [1] power relay 1 */ -#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* [1] power relay 2 */ -#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* [1] accessory power 1 */ -#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* [1] accessory power 2 */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* [1] battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ #define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ enum { /* DSM bind states */ dsm_bind_power_down = 0, diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9606faa86..3f241d29c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -146,7 +146,11 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, }; @@ -570,29 +574,17 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Coefficients here derived by measurement of the 5-16V * range on one unit: * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * XXX pending measurements * - * slope = 0.0046067 - * intercept = 0.3863 + * slope = xxx + * intercept = xxx * - * Intercept corrected for best results @ 12V. + * Intercept corrected for best results @ 5.0V. */ unsigned counts = adc_measure(ADC_VSERVO); if (counts != 0xffff) { unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; } -- cgit v1.2.3 From 4e5eb9740b509e814e9c16aefe40a373d67bbc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 14:50:27 +0200 Subject: Fixed led and reboot linker challenges in C++ environments --- makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/blinkm/blinkm.cpp | 14 +++++++++----- src/drivers/drv_led.h | 2 +- src/drivers/led/led.cpp | 2 +- src/modules/commander/state_machine_helper.c | 2 +- src/modules/systemlib/systemlib.c | 4 ++++ src/modules/systemlib/systemlib.h | 6 +++--- src/systemcmds/reboot/reboot.c | 2 +- 8 files changed, 21 insertions(+), 12 deletions(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 9b3013a5b..98fd6feda 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -12,6 +12,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test # MODULES += drivers/device MODULES += drivers/stm32 +MODULES += drivers/led MODULES += drivers/boards/px4fmuv2 MODULES += systemcmds/perf MODULES += systemcmds/reboot diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a5..8d2eb056e 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -92,7 +92,10 @@ #include +__BEGIN_DECLS #include +__END_DECLS +#include #include #include @@ -112,7 +115,6 @@ #include #include -#include #include #include #include @@ -486,15 +488,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { @@ -831,6 +833,8 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); fprintf(stderr, "options:\n"); diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 21044f620..97f2db107 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -60,6 +60,6 @@ __BEGIN_DECLS /* * Initialise the LED driver. */ -__EXPORT extern void drv_led_start(void); +__EXPORT void drv_led_start(void); __END_DECLS diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 27e43b245..fe307223f 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -117,4 +117,4 @@ drv_led_start(void) if (gLED != nullptr) gLED->init(); } -} \ No newline at end of file +} diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bb..c26478785 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -141,7 +141,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); - up_systemreset(); + systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 3283aad8a..a2b0d8cae 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -50,6 +50,10 @@ #include "systemlib.h" +__EXPORT extern void systemreset(void) { + up_systemreset(); +} + static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e52..77fdfe08a 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +__EXPORT void systemreset(void) noreturn_function; + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 47d3cd948..0fd1e2724 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -47,7 +47,7 @@ __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - up_systemreset(); + systemreset(); } -- cgit v1.2.3 From 87cb066bed1eb9ddb36b233964a77403e0c2ba09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 14:50:45 +0200 Subject: Disabled serial port renumbering --- nuttx-configs/px4fmu-v2/nsh/defconfig | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 70500e4b0..2e73a5a07 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -94,6 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_SDIO_DMA=y # CONFIG_SDIO_DMAPRIO is not set # CONFIG_SDIO_WIDTH_D1_ONLY is not set -- cgit v1.2.3 From 1410625dea4204f758f42c1bdd06959f75a86ef9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Jul 2013 16:12:40 +0200 Subject: Enabled additional drivers on v2, kill misplaced code in mkblctrl --- makefiles/config_px4fmu-v2_default.mk | 5 ++++- src/drivers/mkblctrl/mkblctrl.cpp | 38 ----------------------------------- 2 files changed, 4 insertions(+), 39 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 73dc8bd9d..4add744d0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -21,7 +21,6 @@ MODULES += drivers/boards/px4fmuv2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 -#MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx @@ -30,6 +29,10 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/mkblctrl +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed MODULES += modules/sensors # diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index e78d96569..1f4a63cf3 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1345,44 +1345,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, gpio_bits = 0; servo_mode = MK::MODE_NONE; - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = MK::MODE_4PWM; - break; - - case PORT_GPIO_AND_SERIAL: - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - break; - } - - /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) - g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); -- cgit v1.2.3 From 8c1067a017714394955892e3159c8e0c61bd4ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Jul 2013 23:12:16 +0400 Subject: State machine rewritten, compiles, WIP --- src/modules/commander/commander.cpp | 1178 +++++++++++------------- src/modules/commander/state_machine_helper.cpp | 797 +++++++--------- src/modules/commander/state_machine_helper.h | 12 +- src/modules/mavlink/mavlink.c | 53 +- src/modules/uORB/topics/vehicle_status.h | 55 +- 5 files changed, 928 insertions(+), 1167 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1978d8782..b2336cbf6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -177,6 +177,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int */ int commander_thread_main(int argc, char *argv[]); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); + +transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -198,11 +204,11 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -242,244 +248,171 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: + case VEHICLE_CMD_DO_SET_MODE: + break; - /* request to activate HIL */ - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + break; - if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + break; - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - break; + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - /* request to disarm */ - } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } - } - - break; - - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - /* request for an autopilot reboot */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { - /* reboot is executed later, after positive tune is sent */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - tune_positive(); - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - tune_negative(); - } - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - break; + } - // /* request to land */ - // case VEHICLE_CMD_NAV_LAND: - // { - // //TODO: add check if landing possible - // //TODO: add landing maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } } - // break; - // - // /* request to takeoff */ - // case VEHICLE_CMD_NAV_TAKEOFF: - // { - // //TODO: add check if takeoff possible - // //TODO: add takeoff maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } - // } - // break; - // - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + result = VEHICLE_CMD_RESULT_DENIED; } - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { - /* accel calibration */ - if ((int)(cmd->param5) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + result = VEHICLE_CMD_RESULT_DENIED; } - } - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { + break; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; + case VEHICLE_CMD_PREFLIGHT_STORAGE: - case VEHICLE_CMD_PREFLIGHT_STORAGE: + if (((int)(cmd->param1)) == 0) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; - if (((int)(cmd->param1)) == 0) { + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else if (((int)(cmd->param1)) == 1) { - } else if (((int)(cmd->param1)) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } + break; default: @@ -546,12 +479,12 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + /* Main state machine */ - struct vehicle_status_s current_status; + struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); + memset(&status, 0, sizeof(status)); /* armed topic */ struct actuator_armed_s armed; @@ -566,17 +499,18 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); - current_status.navigation_state = NAVIGATION_STATE_INIT; - current_status.arming_state = ARMING_STATE_INIT; - current_status.hil_state = HIL_STATE_OFF; + status.main_state = MAIN_STATE_MANUAL; + status.navigation_state = NAVIGATION_STATE_STANDBY; + status.arming_state = ARMING_STATE_INIT; + status.hil_state = HIL_STATE_OFF; /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; + status.offboard_control_signal_found_once = false; + status.rc_signal_found_once = false; /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; + status.rc_signal_lost = true; + status.offboard_control_signal_lost = true; /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; @@ -585,31 +519,31 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - + status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; // XXX for now just set sensors as initialized - current_status.condition_system_sensors_initialized = true; + status.condition_system_sensors_initialized = true; // XXX just disable offboard control for now control_mode.flag_control_offboard_enabled = false; /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - + /* publish the new state */ - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ orb_advert_t home_pub = -1; @@ -686,7 +620,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&local_position, 0, sizeof(local_position)); uint64_t last_local_position_time = 0; - /* + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. @@ -747,23 +681,28 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + if (param_get(_param_sys_type, &(status.system_type)) != OK) { warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + if (status.system_type == VEHICLE_TYPE_COAXIAL || + status.system_type == VEHICLE_TYPE_HELICOPTER || + status.system_type == VEHICLE_TYPE_TRICOPTER || + status.system_type == VEHICLE_TYPE_QUADROTOR || + status.system_type == VEHICLE_TYPE_HEXAROTOR || + status.system_type == VEHICLE_TYPE_OCTOROTOR) { control_mode.flag_external_manual_override_ok = false; + status.is_rotary_wing = true; } else { control_mode.flag_external_manual_override_ok = true; + status.is_rotary_wing = false; } /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); + param_get(_param_system_id, &(status.system_id)); + param_get(_param_component_id, &(status.component_id)); } } @@ -800,7 +739,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -830,34 +769,37 @@ int commander_thread_main(int argc, char *argv[]) /* set the condition to valid if there has recently been a local position estimate */ if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; + } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* update battery status */ orb_check(battery_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - current_status.battery_voltage = battery.voltage_v; - current_status.condition_battery_voltage_valid = true; + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - + } - if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { - current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } else { - current_status.battery_voltage = 0.0f; + status.battery_voltage = 0.0f; } /* update subsystem */ orb_check(subsys_sub, &new_data); - + if (new_data) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); @@ -865,26 +807,26 @@ int commander_thread_main(int argc, char *argv[]) /* mark / unmark as present */ if (info.present) { - current_status.onboard_control_sensors_present |= info.subsystem_type; + status.onboard_control_sensors_present |= info.subsystem_type; } else { - current_status.onboard_control_sensors_present &= ~info.subsystem_type; + status.onboard_control_sensors_present &= ~info.subsystem_type; } /* mark / unmark as enabled */ if (info.enabled) { - current_status.onboard_control_sensors_enabled |= info.subsystem_type; + status.onboard_control_sensors_enabled |= info.subsystem_type; } else { - current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + status.onboard_control_sensors_enabled &= ~info.subsystem_type; } /* mark / unmark as ok */ if (info.ok) { - current_status.onboard_control_sensors_health |= info.subsystem_type; + status.onboard_control_sensors_health |= info.subsystem_type; } else { - current_status.onboard_control_sensors_health &= ~info.subsystem_type; + status.onboard_control_sensors_health &= ~info.subsystem_type; } } @@ -899,6 +841,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { /* ready to arm */ led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); @@ -908,7 +851,7 @@ int commander_thread_main(int argc, char *argv[]) /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { /* GPS lock */ led_on(LED_BLUE); @@ -940,20 +883,20 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } - + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; tune_low_bat(); } @@ -961,11 +904,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Critical, this is rather an emergency, change state machine */ - else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; tune_critical_bat(); // XXX implement state change here } @@ -980,9 +923,11 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (current_status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + //TODO publish state + } else { // XXX: Add emergency stuff if sensors are lost } @@ -999,32 +944,32 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.condition_global_position_valid; - bool local_pos_valid = current_status.condition_local_position_valid; - bool airspeed_valid = current_status.condition_airspeed_valid; + bool global_pos_valid = status.condition_global_position_valid; + bool local_pos_valid = status.condition_local_position_valid; + bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { - current_status.condition_global_position_valid = true; + status.condition_global_position_valid = true; } else { - current_status.condition_global_position_valid = false; + status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* Check for valid airspeed/differential pressure measurements */ if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_airspeed_valid = true; + status.condition_airspeed_valid = true; } else { - current_status.condition_airspeed_valid = false; + status.condition_airspeed_valid = false; } /* @@ -1040,9 +985,9 @@ int commander_thread_main(int argc, char *argv[]) // } /* consolidate state change, flag as changed if required */ - if (global_pos_valid != current_status.condition_global_position_valid || - local_pos_valid != current_status.condition_local_position_valid || - airspeed_valid != current_status.condition_airspeed_valid) { + if (global_pos_valid != status.condition_global_position_valid || + local_pos_valid != status.condition_local_position_valid || + airspeed_valid != status.condition_airspeed_valid) { state_changed = true; } @@ -1059,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) // if (!current_status.flag_valid_launch_position && // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it + // first time a valid position, store it and emit it // // XXX implement storage and publication of RTL position // current_status.flag_valid_launch_position = true; @@ -1068,6 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) // } orb_check(gps_sub, &new_data); + if (new_data) { @@ -1093,11 +1039,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !armed.armed) { + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1114,6 +1060,7 @@ int commander_thread_main(int argc, char *argv[]) /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); + } else { home_pub = orb_advertise(ORB_ID(home_position), &home); } @@ -1125,326 +1072,136 @@ int commander_thread_main(int argc, char *argv[]) } /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* start RC state check */ if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - - warnx("mode sw not finite"); - /* no valid stick position, go to default */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, go to manual mode */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.mode_switch = MODE_SWITCH_AUTO; + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); } else { - - /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_ASSISTED; + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + status.rc_signal_cutting_off = false; + status.rc_signal_lost = false; + status.rc_signal_lost_interval = 0; - /* - * Check if land/RTL is requested - */ - if (!isfinite(sp_man.return_switch)) { + transition_result_t res; // store all transitions results here - /* this switch is not properly mapped, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; + /* arm/disarm by RC */ + bool arming_state_changed; - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { + res = TRANSITION_NOT_CHANGED; - /* top stick position */ - current_status.return_switch = RETURN_SWITCH_RETURN; + /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); - } else { - /* center or bottom stick position, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - } + if (res == TRANSITION_CHANGED) + stick_off_counter = 0; - /* check assisted switch */ - if (!isfinite(sp_man.assisted_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; + } else { + stick_off_counter++; + stick_on_counter = 0; + } - } else { - /* center or bottom stick position, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + } else { + stick_off_counter = 0; + } } - /* check mission switch */ - if (!isfinite(sp_man.mission_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; + /* check if left stick is in lower right position and we're in manual mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + status.main_state == MAIN_STATE_MANUAL) { + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + if (res == TRANSITION_CHANGED) + stick_on_counter = 0; - /* top switch position */ - current_status.mission_switch = MISSION_SWITCH_MISSION; + } else { + stick_on_counter++; + stick_off_counter = 0; + } - } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + } else { + stick_on_counter = 0; + } + } - /* bottom switch position */ - current_status.mission_switch = MISSION_SWITCH_NONE; + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } else { + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } - /* center switch position, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + tune_positive(); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } - /* Now it's time to handle the stick inputs */ - - switch (current_status.arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* just manual, XXX this might depend on the return switch */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - - /* Try assisted or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - - /* Try auto or fallback to seatbelt or even manual */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - /* MANUAL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - /* ASSISTED */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* ASSISTED_DESCENT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - } else { - if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { - /* ASSISTED_SIMPLE */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* ASSISTED_SEATBELT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - /* AUTO */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* AUTO_RTL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_DESCENT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - if (current_status.mission_switch == MISSION_SWITCH_MISSION) { - /* AUTO_MISSION */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - // TODO check this - if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* AUTO_LOITER */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - } - } - } - break; - - // XXX we might be missing something that triggers a transition from RTL to LAND - - case ARMING_STATE_ARMED_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); - } else { - if (current_status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } + /* evaluate the main state machine */ + res = check_main_state_machine(&status); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } - /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - * TODO allow disarm when landed - */ - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && - control_mode.flag_control_manual_enabled && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - stick_off_counter = 0; + bool main_state_changed = (res == TRANSITION_CHANGED); - } else { - stick_off_counter++; - stick_on_counter = 0; - } + /* evaluate the navigation state machine */ + res = check_navigation_state_machine(&status, &control_mode); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } - /* check if left stick is in lower right position and we're in manual mode --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - stick_on_counter = 0; - tune_positive(); + bool navigation_state_changed = (res == TRANSITION_CHANGED); - } else { - stick_on_counter++; - stick_off_counter = 0; - } + if (arming_state_changed || main_state_changed || navigation_state_changed) { + /* publish new vehicle status */ + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); } - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; + status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - if (!current_status.rc_signal_cutting_off) { + if (!status.rc_signal_cutting_off) { printf("Reason: not rc_signal_cutting_off\n"); + } else { printf("last print time: %llu\n", last_print_time); } @@ -1453,12 +1210,12 @@ int commander_thread_main(int argc, char *argv[]) } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; + if (status.rc_signal_lost_interval > 1000000) { + status.rc_signal_lost = true; + status.failsave_lowlevel = true; state_changed = true; } @@ -1477,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[]) /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // /* decide about attitude control flag, enable in att/pos/vel */ @@ -1518,42 +1275,44 @@ int commander_thread_main(int argc, char *argv[]) // } // } - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; + status.offboard_control_signal_weak = false; + status.offboard_control_signal_lost = false; + status.offboard_control_signal_lost_interval = 0; // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); + // TODO publish state } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + // TODO publish state } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + if (status.offboard_control_signal_lost_interval > 100000) { + status.offboard_control_signal_lost = true; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; + if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + status.failsave_lowlevel = true; state_changed = true; } } @@ -1563,8 +1322,8 @@ int commander_thread_main(int argc, char *argv[]) - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); + status.counter++; + status.timestamp = hrt_absolute_time(); // XXX this is missing @@ -1580,36 +1339,39 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); state_changed = false; } /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.battery_voltage; + voltage_previous = status.battery_voltage; /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + /* Trigger audio event for low battery */ + + } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + + } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; - } else if(battery_tune_played) { + + } else if (battery_tune_played) { tune_stop(); battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1647,6 +1409,172 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +{ + /* main mode switch */ + if (!isfinite(sp_man->mode_switch)) { + warnx("mode sw not finite"); + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_AUTO; + + } else { + current_status->mode_switch = MODE_SWITCH_MANUAL; + } + + /* land switch */ + if (!isfinite(sp_man->return_switch)) { + current_status->return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { + current_status->return_switch = RETURN_SWITCH_RETURN; + + } else { + current_status->return_switch = RETURN_SWITCH_NONE; + } + + /* assisted switch */ + if (!isfinite(sp_man->assisted_switch)) { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { + current_status->assisted_switch = ASSISTED_SWITCH_EASY; + + } else { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + + /* mission switch */ + if (!isfinite(sp_man->mission_switch)) { + current_status->mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { + current_status->mission_switch = MISSION_SWITCH_NONE; + + } else { + current_status->mission_switch = MISSION_SWITCH_MISSION; + } +} + +transition_result_t +check_main_state_machine(struct vehicle_status_s *current_status) +{ + /* evaluate the main state machine */ + transition_result_t res = TRANSITION_DENIED; + + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT + } + + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT (EASY likely will not work too) + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } + + return res; +} + +transition_result_t +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t res = TRANSITION_DENIED; + + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + /* ARMED */ + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + break; + + case MAIN_STATE_AUTO: + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + } else { + /* if not landed: act depending on switches */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + } + } + } + } + + break; + + default: + break; + } + + } else { + /* DISARMED */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + } + + return res; +} void *commander_low_prio_loop(void *arg) { @@ -1657,72 +1585,76 @@ void *commander_low_prio_loop(void *arg) switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + case LOW_PRIO_TASK_PARAM_LOAD: - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); - } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); - case LOW_PRIO_TASK_PARAM_SAVE: + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); - } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case LOW_PRIO_TASK_PARAM_SAVE: - do_gyro_calibration(mavlink_fd); + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } - case LOW_PRIO_TASK_MAG_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_mag_calibration(mavlink_fd); + case LOW_PRIO_TASK_GYRO_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_gyro_calibration(mavlink_fd); - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - // do_baro_calibration(mavlink_fd); + case LOW_PRIO_TASK_MAG_CALIBRATION: - case LOW_PRIO_TASK_RC_CALIBRATION: + do_mag_calibration(mavlink_fd); - // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; + break; - low_prio_task = LOW_PRIO_TASK_NONE; - break; + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + // do_baro_calibration(mavlink_fd); - do_accel_calibration(mavlink_fd); + case LOW_PRIO_TASK_RC_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + // do_rc_calibration(mavlink_fd); - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_airspeed_calibration(mavlink_fd); + case LOW_PRIO_TASK_ACCEL_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_accel_calibration(mavlink_fd); - case LOW_PRIO_TASK_NONE: - default: - /* slow down to 10Hz */ - usleep(100000); - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4a7aa66b7..043ccfd0c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,102 +62,110 @@ #endif static const int ERROR = -1; -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; +transition_result_t +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + } else { switch (new_arming_state) { - case ARMING_STATE_INIT: + case ARMING_STATE_INIT: - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED) { - - /* sensors need to be initialized for STANDBY state */ - if (current_state->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + break; - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: + case ARMING_STATE_STANDBY: - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED) { - ret = OK; + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; armed->armed = false; - armed->ready_to_arm = false; + armed->ready_to_arm = true; + } else { + mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } - break; - case ARMING_STATE_IN_AIR_RESTORE: + } - /* XXX implement */ - break; - default: - break; - } + break; + + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + break; - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + case ARMING_STATE_STANDBY_ERROR: + + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + hrt_abstime t = hrt_absolute_time(); + status->arming_state = new_arming_state; + armed->timestamp = t; } } @@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta } - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + } else { - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: - - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { - - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_MANUAL_STANDBY: - - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - case NAVIGATION_STATE_MANUAL: + case MAIN_STATE_SEATBELT: - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; - - case NAVIGATION_STATE_ASSISTED_STANDBY: - - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { - - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SEATBELT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SIMPLE: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; + /* need position estimate */ + // TODO only need altitude estimate really + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_AUTO_TAKEOFF: + } else { + ret = TRANSITION_CHANGED; + } - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + break; - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; + case MAIN_STATE_EASY: - default: - break; - } + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); + tune_negative(); + + } else { + ret = TRANSITION_CHANGED; + } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + break; + + case MAIN_STATE_AUTO: + + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); + tune_negative(); + + } else { + ret = TRANSITION_CHANGED; + } + + break; + } - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; } } - + return ret; +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_STANDBY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + current_status->navigation_state = new_navigation_state; + } + } return ret; } @@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s switch (new_state) { - case HIL_STATE_OFF: + case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } - case HIL_STATE_ON: + break; - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + case HIL_STATE_ON: - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } - default: - warnx("Unknown hil state"); - break; + break; + + default: + warnx("Unknown hil state"); + break; } } @@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); ret = OK; + } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 75fdd224c..b59b66c8b 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -49,10 +49,18 @@ #include #include +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); +} transition_result_t; -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ae8e168e1..9132d1b49 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } @@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { + if (v_status.main_state == MAIN_STATE_MANUAL + || v_status.main_state == MAIN_STATE_SEATBELT + || v_status.main_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { + if (v_status.navigation_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* set calibration state */ if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - - // XXX difference between active and armed? is AUTO_READY active? - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - + } else if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { - + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { - - *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; } else { - warnx("Unknown mavlink state"); *mavlink_state = MAV_STATE_CRITICAL; } - } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9b91e834e..e7feaa98e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -58,29 +58,28 @@ * @addtogroup topics @{ */ -/* State Machine */ +/* main state machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_MANUAL_STANDBY, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_ASSISTED_STANDBY, - NAVIGATION_STATE_ASSISTED_SEATBELT, - NAVIGATION_STATE_ASSISTED_SIMPLE, - NAVIGATION_STATE_ASSISTED_DESCENT, - NAVIGATION_STATE_AUTO_STANDBY, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_AUTO_TAKEOFF, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_LAND -} navigation_state_t; + MAIN_STATE_MANUAL = 0, + MAIN_STATE_SEATBELT, + MAIN_STATE_EASY, + MAIN_STATE_AUTO, +} main_state_t; +/* navigation state machine */ typedef enum { - MANUAL_STANDBY = 0, - MANUAL_READY, - MANUAL_IN_AIR -} manual_state_t; + NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed + NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_STABILIZE, // attitude stabilization + NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization + NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization + NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff + NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode + NAVIGATION_STATE_AUTO_LOITER, // pause mission + NAVIGATION_STATE_AUTO_MISSION, // fly mission + NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND + NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) +} navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, @@ -103,16 +102,16 @@ typedef enum { MODE_SWITCH_AUTO } mode_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_EASY +} assisted_switch_pos_t; + typedef enum { RETURN_SWITCH_NONE = 0, RETURN_SWITCH_RETURN } return_switch_pos_t; -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_SIMPLE -} assisted_switch_pos_t; - typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -175,7 +174,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current system state */ + main_state_t main_state; /**< main state machine */ + navigation_state_t navigation_state; /**< navigation state machine */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ @@ -183,6 +183,8 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ + bool is_rotary_wing; + mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; @@ -198,6 +200,7 @@ struct vehicle_status_s bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ -- cgit v1.2.3 From 57cbf724f159cec88b21281a4ace415276df3a38 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 29 Jul 2013 23:13:46 -0700 Subject: Fix the clock enable register for FMUv2 PWM outputs 1-4. Teach the stm32 pwm driver about the MOE bit on advanced timers. --- src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 2 +- src/drivers/stm32/drv_pwm_servo.c | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c index d1656005b..bcbc0010c 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c @@ -53,7 +53,7 @@ __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { .base = STM32_TIM1_BASE, - .clock_register = STM32_RCC_APB1ENR, + .clock_register = STM32_RCC_APB2ENR, .clock_bit = RCC_APB2ENR_TIM1EN, .clock_freq = STM32_APB2_TIM1_CLKIN }, diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index 7b060412c..c85e83ddc 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -88,6 +88,7 @@ #define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) static void pwm_timer_init(unsigned timer); static void pwm_timer_set_rate(unsigned timer, unsigned rate); @@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer) rCCER(timer) = 0; rDCR(timer) = 0; + if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) { + /* master output enable = on */ + rBDTR(timer) = ATIM_BDTR_MOE; + } + /* configure the timer to free-run at 1MHz */ rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; @@ -163,6 +169,9 @@ pwm_channel_init(unsigned channel) rCCER(timer) |= GTIM_CCER_CC4E; break; } + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; } int @@ -203,6 +212,9 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) return -1; } + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + return 0; } -- cgit v1.2.3 From 7e9a18da795e56e229957dba47ed7468eac10697 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 30 Jul 2013 15:10:22 +0200 Subject: Changed gyro scaling according to datasheet --- src/drivers/l3gd20/l3gd20.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a9af6711b..423adc76d 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -559,28 +559,32 @@ int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; - if (max_dps == 0) + if (max_dps == 0) { max_dps = 2000; - + } if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); return OK; -- cgit v1.2.3 From b85d74336d62d467b21f98f69020c5db2f21efb0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 30 Jul 2013 22:46:41 -0700 Subject: Add missing 'fi' at the end of rc script; if you had a microSD card that set MODE to something other than autostart the result was a prompt that ignored your commands. --- ROMFS/px4fmu_common/init.d/rcS | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6d141f1b6..ccbae2cbc 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -119,3 +119,6 @@ if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom fi + +# End of autostart +fi -- cgit v1.2.3 From feee1ccc65dff865270d22ff6796b6acedf67ea2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 30 Jul 2013 22:47:17 -0700 Subject: Remove some timer reload events; these seem to break PWM output on IO. --- src/drivers/stm32/drv_pwm_servo.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index c85e83ddc..dbb45a138 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -169,9 +169,6 @@ pwm_channel_init(unsigned channel) rCCER(timer) |= GTIM_CCER_CC4E; break; } - - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; } int @@ -212,9 +209,6 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) return -1; } - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; - return 0; } -- cgit v1.2.3 From 02d4480e8ed7c6c6460f95f531aeef2725951663 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 31 Jul 2013 20:58:27 +0400 Subject: commander rewrite almost completed, WIP --- src/modules/commander/commander.cpp | 843 +++++++++++-------------- src/modules/commander/commander_helper.cpp | 6 +- src/modules/commander/state_machine_helper.cpp | 70 +- src/modules/commander/state_machine_helper.h | 12 +- 4 files changed, 445 insertions(+), 486 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b2336cbf6..2012abcc0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -73,7 +73,6 @@ #include #include #include -#include #include #include @@ -124,6 +123,26 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define PRINT_INTERVAL 5000000 +#define PRINT_MODE_REJECT_INTERVAL 2000000 + +// TODO include mavlink headers +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +enum MAV_MODE_FLAG { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END = 129, /* | */ +}; +#endif + /* Mavlink file descriptors */ static int mavlink_fd; @@ -135,9 +154,13 @@ static int daemon_task; /**< Handle of daemon task / thread */ /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +static unsigned int leds_counter; +/* To remember when last notification was sent */ +static uint64_t last_print_mode_reject_time = 0; + /* tasks waiting for low prio thread */ -enum { +typedef enum { LOW_PRIO_TASK_NONE = 0, LOW_PRIO_TASK_PARAM_SAVE, LOW_PRIO_TASK_PARAM_LOAD, @@ -147,8 +170,9 @@ enum { LOW_PRIO_TASK_RC_CALIBRATION, LOW_PRIO_TASK_ACCEL_CALIBRATION, LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task; +} low_prio_task_t; +static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; /** * The daemon app only briefly exists to start @@ -170,17 +194,21 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. */ int commander_thread_main(int argc, char *argv[]); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +void print_reject_mode(const char *msg); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -241,141 +269,112 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - break; + case VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (int) cmd->param1; + uint8_t custom_mode = (int) cmd->param2; + // TODO remove debug code + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + /* set arming state */ + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); + } - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - break; + } else { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); + } + } else { + arming_res = TRANSITION_NOT_CHANGED; + } + } - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + if (custom_mode & 1) { // TODO add custom mode flags definition + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else { - result = VEHICLE_CMD_RESULT_DENIED; + } else { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } } + } + + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { + break; + } - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + break; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + break; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } + if ((int)(cmd->param1) == 1) { + /* gyro calibration */ + new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if ((int)(cmd->param2) == 1) { + /* magnetometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else if ((int)(cmd->param3) == 1) { + /* zero-altitude pressure calibration */ + //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else if ((int)(cmd->param4) == 1) { + /* RC calibration */ + new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + } else if ((int)(cmd->param5) == 1) { + /* accelerometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else if ((int)(cmd->param6) == 1) { + /* airspeed calibration */ + new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; } - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state + if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -384,54 +383,55 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - break; + break; + } - case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - if (((int)(cmd->param1)) == 0) { - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + if (((int)(cmd->param1)) == 0) { low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else if (((int)(cmd->param1)) == 1) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { result = VEHICLE_CMD_RESULT_ACCEPTED; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - break; + break; + } default: - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; break; } /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED || - result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + } else { tune_negative(); - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); - tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); + } } /* send any requested ACKs */ @@ -515,16 +515,9 @@ int commander_thread_main(int argc, char *argv[]) /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - // current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ - /* XXX do we need this? */ - //current_status.flag_safety_present = false; - // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -535,12 +528,11 @@ int commander_thread_main(int argc, char *argv[]) status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - /* publish the new state */ + /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -556,8 +548,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } - // XXX needed? - mavlink_log_info(mavlink_fd, "system is running"); + mavlink_log_info(mavlink_fd, "[cmd] started"); pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); @@ -577,9 +568,10 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_on_counter = 0; /* To remember when last notification was sent */ - uint64_t last_print_time = 0; + uint64_t last_print_control_time = 0; - float voltage_previous = 0.0f; + enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; + bool armed_previous = false; bool low_battery_voltage_actions_done; bool critical_battery_voltage_actions_done; @@ -588,10 +580,10 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = 0; - bool state_changed = true; + bool status_changed = true; bool param_init_forced = true; - bool new_data = false; + bool updated = false; /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -612,13 +604,11 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; /* * The home position is set based on GPS only, to prevent a dependency between @@ -670,11 +660,12 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { + hrt_abstime t = hrt_absolute_time(); /* update parameters */ - orb_check(param_changed_sub, &new_data); + orb_check(param_changed_sub, &updated); - if (new_data || param_init_forced) { + if (updated || param_init_forced) { param_init_forced = false; /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); @@ -703,94 +694,96 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - + status_changed = true; } } - orb_check(sp_man_sub, &new_data); + orb_check(sp_man_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &new_data); + orb_check(sp_offboard_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(sensor_sub, &new_data); + orb_check(sensor_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } - orb_check(diff_pres_sub, &new_data); + orb_check(diff_pres_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); last_diff_pres_time = diff_pres.timestamp; } - orb_check(cmd_sub, &new_data); + orb_check(cmd_sub, &updated); - if (new_data) { + if (updated) { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(&status, &control_mode, &cmd, &armed); } /* update safety topic */ - orb_check(safety_sub, &new_data); + orb_check(safety_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ - orb_check(global_position_sub, &new_data); + orb_check(global_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; } /* update local position estimate */ - orb_check(local_position_sub, &new_data); + orb_check(local_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; } /* set the condition to valid if there has recently been a local position estimate */ - if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - status.condition_local_position_valid = true; + if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (!status.condition_local_position_valid) { + status.condition_local_position_valid = true; + status_changed = true; + } } else { - status.condition_local_position_valid = false; + if (status.condition_local_position_valid) { + status.condition_local_position_valid = false; + status_changed = true; + } } /* update battery status */ - orb_check(battery_sub, &new_data); + orb_check(battery_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - } - if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); } else { @@ -798,12 +791,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update subsystem */ - orb_check(subsys_sub, &new_data); + orb_check(subsys_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("Subsys changed: %d\n", (int)info.subsystem_type); + warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { @@ -828,56 +821,12 @@ int commander_thread_main(int argc, char *argv[]) } else { status.onboard_control_sensors_health &= ~info.subsystem_type; } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - - /* XXX if armed */ - if (armed.armed) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { - /* ready to arm */ - led_toggle(LED_AMBER); - - } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not ready to arm, something is wrong */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - - // /* toggle GPS led at 5 Hz in HIL mode */ - // if (current_status.flag_hil_enabled) { - // /* hil enabled */ - // led_toggle(LED_BLUE); - - // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - // /* toggle arming (red) at 5 Hz on low battery or error */ - // led_toggle(LED_AMBER); - // } + status_changed = true; } + toggle_status_leds(&status, &armed, &gps_position); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -888,29 +837,26 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - tune_low_bat(); + status_changed = true; } low_voltage_counter++; - } - /* Critical, this is rather an emergency, change state machine */ - else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - tune_critical_bat(); - // XXX implement state change here + arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + status_changed = true; } critical_voltage_counter++; @@ -923,10 +869,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - //TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -950,14 +895,14 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { + if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { status.condition_global_position_valid = true; } else { status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { + if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { status.condition_local_position_valid = true; } else { @@ -965,66 +910,23 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + if (t - last_diff_pres_time < 2000000 && t > 2000000) { status.condition_airspeed_valid = true; } else { status.condition_airspeed_valid = false; } - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - // if (current_status.condition_local_position_valid || - // current_status.condition_global_position_valid) { - // current_status.flag_vector_flight_mode_ok = true; - - // } else { - // current_status.flag_vector_flight_mode_ok = false; - // } - - /* consolidate state change, flag as changed if required */ - if (global_pos_valid != status.condition_global_position_valid || - local_pos_valid != status.condition_local_position_valid || - airspeed_valid != status.condition_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - // if (!current_status.flag_valid_launch_position && - // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it - - // // XXX implement storage and publication of RTL position - // current_status.flag_valid_launch_position = true; - // current_status.flag_auto_flight_mode_ok = true; - // state_changed = true; - // } - - orb_check(gps_sub, &new_data); - - if (new_data) { - + orb_check(gps_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ float hdop_m = gps_position.eph_m; float vdop_m = gps_position.epv_m; - /* check if gps fix is ok */ - // XXX magic number + /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -1038,15 +940,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (t - gps_position.timestamp_position < 2000000) && !armed.armed) { - warnx("setting home position"); - /* copy position data to uORB home message, store it locally as well */ + // TODO use global position estimate home.lat = gps_position.lat; home.lon = gps_position.lon; home.alt = gps_position.alt; @@ -1057,6 +956,11 @@ int commander_thread_main(int argc, char *argv[]) home.s_variance_m_s = gps_position.s_variance_m_s; home.p_variance_m = gps_position.p_variance_m; + double home_lat_d = home.lat * 1e-7; + double home_lon_d = home.lon * 1e-7; + warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1073,16 +977,18 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC state check */ - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + /* start RC input check */ + if ((t - sp_man.timestamp) < 100000) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + status_changed = true; } } @@ -1093,8 +999,6 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res; // store all transitions results here /* arm/disarm by RC */ - bool arming_state_changed; - res = TRANSITION_NOT_CHANGED; /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm @@ -1106,16 +1010,15 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_off_counter = 0; + res = arming_state_transition(&status, new_arming_state, &armed); + stick_off_counter = 0; } else { stick_off_counter++; - stick_on_counter = 0; } + stick_on_counter = 0; + } else { stick_off_counter = 0; } @@ -1126,16 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_on_counter = 0; + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { stick_on_counter++; - stick_off_counter = 0; } + stick_off_counter = 0; + } else { stick_on_counter = 0; } @@ -1148,9 +1050,6 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - - tune_positive(); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* fill current_status according to mode switches */ @@ -1159,83 +1058,54 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine */ res = check_main_state_machine(&status); - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } - - bool main_state_changed = (res == TRANSITION_CHANGED); - - /* evaluate the navigation state machine */ - res = check_navigation_state_machine(&status, &control_mode); - - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - } - - bool navigation_state_changed = (res == TRANSITION_CHANGED); - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - /* publish new vehicle status */ - status.counter++; - status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); - mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - } + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); - if (navigation_state_changed) { - /* publish new navigation state */ - control_mode.counter++; - control_mode.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - + /* print error message for first RC glitch and then every 5s */ + if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + // TODO remove debug code if (!status.rc_signal_cutting_off) { - printf("Reason: not rc_signal_cutting_off\n"); + warnx("Reason: not rc_signal_cutting_off\n"); } else { - printf("last print time: %llu\n", last_print_time); + warnx("last print time: %llu\n", last_print_control_time); } - last_print_time = hrt_absolute_time(); + /* only complain if the offboard control is NOT active */ + status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); + + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = t - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { status.rc_signal_lost = true; status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } } } - - - - /* End mode switch */ - + /* END mode switch */ /* END RC state check */ - - /* State machine update for offboard control */ + // TODO check this + /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1282,87 +1152,94 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - - arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* print error message for first offboard signal glitch and then every 5s */ + if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); + status.failsave_lowlevel_start_time = t; tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } } } } + /* evaluate the navigation state machine */ + transition_result_t res = check_navigation_state_machine(&status, &control_mode); + if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + } + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = check_arming_state_changed(); + bool main_state_changed = check_main_state_changed(); + bool navigation_state_changed = check_navigation_state_changed(); - status.counter++; - status.timestamp = hrt_absolute_time(); - + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; + } - // XXX this is missing - /* If full run came back clean, transition to standby */ - // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - // current_status.flag_preflight_gyro_calibration == false && - // current_status.flag_preflight_mag_calibration == false && - // current_status.flag_preflight_accel_calibration == false) { - // /* All ok, no calibration going on, go to standby */ - // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - // } + /* publish arming state */ + if (arming_state_changed) { + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + /* publish control mode */ + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + /* publish vehicle status at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + status.counter++; + status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - state_changed = false; + status_changed = false; } - - - /* Store old modes to detect and act on state transitions */ - voltage_previous = status.battery_voltage; - - - /* play tone according to evaluation result */ - /* check if we recently armed */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + /* play arming and battery warning tunes */ + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - - } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { - if (tune_critical_bat() == OK) + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* play tune on battery warning */ + if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { - if (tune_low_bat() == OK) + } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + /* play tune on battery critical */ + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (battery_tune_played) { @@ -1375,8 +1252,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + /* store old modes to detect and act on state transitions */ + battery_warning_previous = status.battery_warning; + armed_previous = armed.armed; - /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -1409,6 +1288,46 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +{ + if (leds_counter % 2 == 0) { + /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 2.5Hz */ + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); + } + + } else { + /* not ready to arm, blink at 10Hz */ + led_toggle(LED_AMBER); + } + + if (status->condition_global_position_valid) { + /* position estimated, solid */ + led_on(LED_BLUE); + + } else if (leds_counter == 0) { + /* waiting for position estimate, short blink at 1.25Hz */ + led_on(LED_BLUE); + + } else { + /* no position estimator available, off */ + led_off(LED_BLUE); + } + } + + leds_counter++; + + if (leds_counter >= 16) + leds_counter = 0; +} + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) { @@ -1420,8 +1339,11 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_AUTO; - } else { + } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else { + current_status->mode_switch = MODE_SWITCH_ASSISTED; } /* land switch */ @@ -1466,44 +1388,49 @@ check_main_state_machine(struct vehicle_status_s *current_status) switch (current_status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT + print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1514,6 +1441,20 @@ check_main_state_machine(struct vehicle_status_s *current_status) return res; } +void +print_reject_mode(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] WARNING: reject %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { @@ -1523,15 +1464,15 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* ARMED */ switch (current_status->main_state) { case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); break; case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); break; case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); break; case MAIN_STATE_AUTO: @@ -1539,7 +1480,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* don't act while taking off */ if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); // TRANSITION_DENIED is not possible here break; @@ -1547,16 +1488,16 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* if not landed: act depending on switches */ if (current_status->return_switch == RETURN_SWITCH_RETURN) { /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { if (current_status->mission_switch == MISSION_SWITCH_MISSION) { /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } @@ -1570,7 +1511,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v } else { /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); } return res; @@ -1579,75 +1520,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void *commander_low_prio_loop(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "commander low prio", getpid()); + prctl(PR_SET_NAME, "commander_low_prio", getpid()); while (!thread_should_exit) { switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1657,6 +1584,8 @@ void *commander_low_prio_loop(void *arg) break; } + low_prio_task = LOW_PRIO_TASK_NONE; + } return 0; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 9427bd892..681a11568 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -213,7 +213,7 @@ float battery_remaining_estimate_voltage(float voltage) ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; + ret = (ret < 0.0f) ? 0.0f : ret; + ret = (ret > 1.0f) ? 1.0f : ret; return ret; -} \ No newline at end of file +} diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 043ccfd0c..06cd060c5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,8 +62,12 @@ #endif static const int ERROR = -1; +static bool arming_state_changed = true; +static bool main_state_changed = true; +static bool navigation_state_changed = true; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -96,9 +100,6 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi ret = TRANSITION_CHANGED; armed->armed = false; armed->ready_to_arm = true; - - } else { - mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } } @@ -163,18 +164,28 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi } if (ret == TRANSITION_CHANGED) { - hrt_abstime t = hrt_absolute_time(); status->arming_state = new_arming_state; - armed->timestamp = t; + arming_state_changed = true; } } return ret; } +bool +check_arming_state_changed() +{ + if (arming_state_changed) { + arming_state_changed = false; + return true; + + } else { + return false; + } +} transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -193,11 +204,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need position estimate */ // TODO only need altitude estimate really - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -206,11 +213,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -219,11 +222,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -232,14 +231,27 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m if (ret == TRANSITION_CHANGED) { current_state->main_state = new_main_state; + main_state_changed = true; } } return ret; } +bool +check_main_state_changed() +{ + if (main_state_changed) { + main_state_changed = false; + return true; + + } else { + return false; + } +} + transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; @@ -395,12 +407,24 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + navigation_state_changed = true; } } return ret; } +bool +check_navigation_state_changed() +{ + if (navigation_state_changed) { + navigation_state_changed = false; + return true; + + } else { + return false; + } +} /** * Transition from one hil state to another diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b59b66c8b..c8c77e5d8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,11 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); -transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); +bool check_arming_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); + +bool check_main_state_changed(); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); + +bool check_navigation_state_changed(); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); -- cgit v1.2.3 From f3764d0b5a219aea24d05274311d91ad08eb182d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 28 Jul 2013 17:20:12 +1000 Subject: px4fmu: expanded "fmu test" this now does testing in a similar manner as "px4io test", except that it tests both ioctl and write based setting of servos --- src/drivers/px4fmu/fmu.cpp | 84 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 70 insertions(+), 14 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 70147d56a..b73f71572 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1104,28 +1104,84 @@ void test(void) { int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) errx(1, "open fail"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); - - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } + + warnx("Testing %u servos", (unsigned)servo_count); + + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + + for (;;) { + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; + + if (direction == 1) { + // use ioctl interface for one direction + for (unsigned i=0; i < servo_count; i++) { + if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { + err(1, "servo %u set failed", i); + } + } + } else { + // and use write interface for the other direction + int ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } + + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - if (ioctl(fd, PWM_SERVO_SET(4), 1800) < 0) err(1, "servo 5 set failed"); + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } - if (ioctl(fd, PWM_SERVO_SET(5), 2000) < 0) err(1, "servo 6 set failed"); -#endif + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + break; + } + } + } + close(console); close(fd); exit(0); @@ -1222,9 +1278,9 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fprintf(stderr, " mode_gpio, mode_pwm\n"); + fprintf(stderr, " mode_gpio, mode_pwm, test\n"); #endif exit(1); } -- cgit v1.2.3 From c7d8535151c336dfbc0bdf522efb358d0c250bd5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 31 Jul 2013 17:53:11 +1000 Subject: px4io: don't try the px4io serial interface on FMUv1 this caused px4io start to fail on FMUv1 --- src/drivers/px4io/px4io.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dd876f903..281debe5c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1659,11 +1659,13 @@ get_interface() { device::Device *interface = nullptr; +#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 /* try for a serial interface */ if (PX4IO_serial_interface != nullptr) interface = PX4IO_serial_interface(); if (interface != nullptr) goto got; +#endif /* try for an I2C interface if we haven't got a serial one */ if (PX4IO_i2c_interface != nullptr) -- cgit v1.2.3 From bee8e27f0479621f5b1ca5e43b4faf1a8f27bfcd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Aug 2013 22:15:16 +1000 Subject: adc: allow "adc test" to read 10 values --- src/drivers/stm32/adc/adc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 48c95b3dd..00e46d6b8 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[8]; + adc_msg_s data[10]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) -- cgit v1.2.3 From b9446af3f9758604211aefe64f91e27a7e71c631 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Aug 2013 16:29:43 +0200 Subject: Show correct battery voltage for v2 boards --- src/modules/sensors/sensor_params.c | 6 +++++- src/modules/sensors/sensors.cpp | 31 +++++++++++++++++++++++++++++-- 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 133cda8d6..ba4d2186c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -157,8 +157,12 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +#endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ff9e19b4..d9185891b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,8 +92,35 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + #define ADC_BATTERY_VOLTAGE_CHANNEL 10 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + #define ADC_BATTERY_VOLTAGE_CHANNEL 2 + #define ADC_BATTERY_CURRENT_CHANNEL 3 + #define ADC_5V_RAIL_SENSE 4 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif #define BAT_VOL_INITIAL 0.f #define BAT_VOL_LOWPASS_1 0.99f -- cgit v1.2.3 From a9c1882ea01aa0cf00448bc874c97087853bb13c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Aug 2013 23:09:40 +1000 Subject: l3gd20: fixed bit definitions for filter rates and allow requests for the rates in table 21 of the l3gd20H datasheet --- src/drivers/l3gd20/l3gd20.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 423adc76d..61a38b125 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -86,8 +86,8 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -598,19 +598,20 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - if (frequency <= 95) { + // use limits good for H or non-H models + if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; - } else if (frequency <= 190) { + } else if (frequency <= 200) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; - } else if (frequency <= 380) { + } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_30HZ; + bits |= RATE_380HZ_LP_20HZ; - } else if (frequency <= 760) { + } else if (frequency <= 800) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; -- cgit v1.2.3 From 9d6ec6b3655fcd902be7a7fe4f2a24033c714afb Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 2 Aug 2013 22:34:55 -0700 Subject: Restructure things so that the PX4 configs move out of the NuttX tree, and most of the PX4-specific board configuration data moves out of the config and into the board driver. Rename some directories that got left behind in the great board renaming. --- Makefile | 2 +- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 7 +- makefiles/config_px4fmu-v2_test.mk | 2 +- makefiles/config_px4io-v1_default.mk | 2 +- makefiles/config_px4io-v2_default.mk | 2 +- makefiles/firmware.mk | 5 + makefiles/setup.mk | 1 + nuttx-configs/px4fmu-v1/Kconfig | 21 + nuttx-configs/px4fmu-v1/common/Make.defs | 184 +++++ nuttx-configs/px4fmu-v1/common/ld.script | 149 ++++ nuttx-configs/px4fmu-v1/include/board.h | 319 ++++++++ nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h | 42 ++ nuttx-configs/px4fmu-v1/nsh/Make.defs | 3 + nuttx-configs/px4fmu-v1/nsh/defconfig | 957 ++++++++++++++++++++++++ nuttx-configs/px4fmu-v1/nsh/setenv.sh | 75 ++ nuttx-configs/px4fmu-v1/ostest/Make.defs | 122 +++ nuttx-configs/px4fmu-v1/ostest/defconfig | 583 +++++++++++++++ nuttx-configs/px4fmu-v1/ostest/setenv.sh | 75 ++ nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld | 129 ++++ nuttx-configs/px4fmu-v1/scripts/ld.script | 123 +++ nuttx-configs/px4fmu-v1/src/Makefile | 84 +++ nuttx-configs/px4fmu-v1/src/empty.c | 4 + nuttx-configs/px4fmu-v1/tools/px_mkfw.py | 110 +++ nuttx-configs/px4fmu-v1/tools/px_uploader.py | 416 ++++++++++ nuttx-configs/px4fmu-v1/tools/upload.sh | 27 + nuttx-configs/px4fmu-v2/include/board.h | 58 -- nuttx-configs/px4io-v1/Kconfig | 21 + nuttx-configs/px4io-v1/README.txt | 806 ++++++++++++++++++++ nuttx-configs/px4io-v1/common/Make.defs | 171 +++++ nuttx-configs/px4io-v1/common/ld.script | 129 ++++ nuttx-configs/px4io-v1/common/setenv.sh | 47 ++ nuttx-configs/px4io-v1/include/README.txt | 1 + nuttx-configs/px4io-v1/include/board.h | 152 ++++ nuttx-configs/px4io-v1/include/drv_i2c_device.h | 42 ++ nuttx-configs/px4io-v1/nsh/Make.defs | 3 + nuttx-configs/px4io-v1/nsh/appconfig | 32 + nuttx-configs/px4io-v1/nsh/defconfig | 559 ++++++++++++++ nuttx-configs/px4io-v1/nsh/setenv.sh | 47 ++ nuttx-configs/px4io-v1/src/Makefile | 84 +++ nuttx-configs/px4io-v1/src/README.txt | 1 + nuttx-configs/px4io-v1/src/drv_i2c_device.c | 618 +++++++++++++++ nuttx-configs/px4io-v1/src/empty.c | 4 + nuttx-configs/px4io-v2/include/board.h | 28 - src/drivers/blinkm/blinkm.cpp | 16 +- src/drivers/bma180/bma180.cpp | 2 +- src/drivers/boards/px4fmu-v1/board_config.h | 209 ++++++ src/drivers/boards/px4fmu-v1/module.mk | 10 + src/drivers/boards/px4fmu-v1/px4fmu_can.c | 144 ++++ src/drivers/boards/px4fmu-v1/px4fmu_init.c | 268 +++++++ src/drivers/boards/px4fmu-v1/px4fmu_led.c | 96 +++ src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c | 87 +++ src/drivers/boards/px4fmu-v1/px4fmu_spi.c | 154 ++++ src/drivers/boards/px4fmu-v1/px4fmu_usb.c | 108 +++ src/drivers/boards/px4fmu-v2/board_config.h | 192 +++++ src/drivers/boards/px4fmu-v2/module.mk | 10 + src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 262 +++++++ src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 85 +++ src/drivers/boards/px4fmu-v2/px4fmu_can.c | 144 ++++ src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c | 105 +++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 141 ++++ src/drivers/boards/px4fmu-v2/px4fmu_usb.c | 108 +++ src/drivers/boards/px4fmu/module.mk | 10 - src/drivers/boards/px4fmu/px4fmu_can.c | 144 ---- src/drivers/boards/px4fmu/px4fmu_init.c | 268 ------- src/drivers/boards/px4fmu/px4fmu_internal.h | 165 ---- src/drivers/boards/px4fmu/px4fmu_led.c | 96 --- src/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 87 --- src/drivers/boards/px4fmu/px4fmu_spi.c | 154 ---- src/drivers/boards/px4fmu/px4fmu_usb.c | 108 --- src/drivers/boards/px4fmuv2/module.mk | 10 - src/drivers/boards/px4fmuv2/px4fmu2_init.c | 262 ------- src/drivers/boards/px4fmuv2/px4fmu2_led.c | 85 --- src/drivers/boards/px4fmuv2/px4fmu_can.c | 144 ---- src/drivers/boards/px4fmuv2/px4fmu_internal.h | 143 ---- src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c | 105 --- src/drivers/boards/px4fmuv2/px4fmu_spi.c | 141 ---- src/drivers/boards/px4fmuv2/px4fmu_usb.c | 108 --- src/drivers/boards/px4io-v1/board_config.h | 95 +++ src/drivers/boards/px4io-v1/module.mk | 6 + src/drivers/boards/px4io-v1/px4io_init.c | 106 +++ src/drivers/boards/px4io-v1/px4io_pwm_servo.c | 123 +++ src/drivers/boards/px4io-v2/board_config.h | 138 ++++ src/drivers/boards/px4io-v2/module.mk | 6 + src/drivers/boards/px4io-v2/px4iov2_init.c | 162 ++++ src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c | 123 +++ src/drivers/boards/px4io/module.mk | 6 - src/drivers/boards/px4io/px4io_init.c | 106 --- src/drivers/boards/px4io/px4io_internal.h | 85 --- src/drivers/boards/px4io/px4io_pwm_servo.c | 123 --- src/drivers/boards/px4iov2/module.mk | 6 - src/drivers/boards/px4iov2/px4iov2_init.c | 162 ---- src/drivers/boards/px4iov2/px4iov2_internal.h | 118 --- src/drivers/boards/px4iov2/px4iov2_pwm_servo.c | 123 --- src/drivers/drv_gpio.h | 8 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/hmc5883/hmc5883.cpp | 2 +- src/drivers/l3gd20/l3gd20.cpp | 3 +- src/drivers/lsm303d/lsm303d.cpp | 4 +- src/drivers/mb12xx/mb12xx.cpp | 4 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 3 +- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 16 +- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/px4io/px4io_uploader.cpp | 7 +- src/drivers/rgbled/rgbled.cpp | 3 +- src/drivers/stm32/drv_hrt.c | 7 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- src/modules/px4iofirmware/px4io.h | 7 +- src/modules/systemlib/systemlib.c | 4 +- src/systemcmds/eeprom/eeprom.c | 2 +- src/systemcmds/i2c/i2c.c | 2 +- 113 files changed, 9075 insertions(+), 2915 deletions(-) create mode 100644 nuttx-configs/px4fmu-v1/Kconfig create mode 100644 nuttx-configs/px4fmu-v1/common/Make.defs create mode 100644 nuttx-configs/px4fmu-v1/common/ld.script create mode 100644 nuttx-configs/px4fmu-v1/include/board.h create mode 100644 nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h create mode 100644 nuttx-configs/px4fmu-v1/nsh/Make.defs create mode 100644 nuttx-configs/px4fmu-v1/nsh/defconfig create mode 100755 nuttx-configs/px4fmu-v1/nsh/setenv.sh create mode 100644 nuttx-configs/px4fmu-v1/ostest/Make.defs create mode 100644 nuttx-configs/px4fmu-v1/ostest/defconfig create mode 100755 nuttx-configs/px4fmu-v1/ostest/setenv.sh create mode 100644 nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld create mode 100644 nuttx-configs/px4fmu-v1/scripts/ld.script create mode 100644 nuttx-configs/px4fmu-v1/src/Makefile create mode 100644 nuttx-configs/px4fmu-v1/src/empty.c create mode 100755 nuttx-configs/px4fmu-v1/tools/px_mkfw.py create mode 100755 nuttx-configs/px4fmu-v1/tools/px_uploader.py create mode 100755 nuttx-configs/px4fmu-v1/tools/upload.sh create mode 100644 nuttx-configs/px4io-v1/Kconfig create mode 100755 nuttx-configs/px4io-v1/README.txt create mode 100644 nuttx-configs/px4io-v1/common/Make.defs create mode 100755 nuttx-configs/px4io-v1/common/ld.script create mode 100755 nuttx-configs/px4io-v1/common/setenv.sh create mode 100755 nuttx-configs/px4io-v1/include/README.txt create mode 100755 nuttx-configs/px4io-v1/include/board.h create mode 100644 nuttx-configs/px4io-v1/include/drv_i2c_device.h create mode 100644 nuttx-configs/px4io-v1/nsh/Make.defs create mode 100644 nuttx-configs/px4io-v1/nsh/appconfig create mode 100755 nuttx-configs/px4io-v1/nsh/defconfig create mode 100755 nuttx-configs/px4io-v1/nsh/setenv.sh create mode 100644 nuttx-configs/px4io-v1/src/Makefile create mode 100644 nuttx-configs/px4io-v1/src/README.txt create mode 100644 nuttx-configs/px4io-v1/src/drv_i2c_device.c create mode 100644 nuttx-configs/px4io-v1/src/empty.c create mode 100644 src/drivers/boards/px4fmu-v1/board_config.h create mode 100644 src/drivers/boards/px4fmu-v1/module.mk create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_init.c create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_led.c create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu-v1/px4fmu_usb.c create mode 100644 src/drivers/boards/px4fmu-v2/board_config.h create mode 100644 src/drivers/boards/px4fmu-v2/module.mk create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu2_init.c create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu2_led.c create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_can.c create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_spi.c create mode 100644 src/drivers/boards/px4fmu-v2/px4fmu_usb.c delete mode 100644 src/drivers/boards/px4fmu/module.mk delete mode 100644 src/drivers/boards/px4fmu/px4fmu_can.c delete mode 100644 src/drivers/boards/px4fmu/px4fmu_init.c delete mode 100644 src/drivers/boards/px4fmu/px4fmu_internal.h delete mode 100644 src/drivers/boards/px4fmu/px4fmu_led.c delete mode 100644 src/drivers/boards/px4fmu/px4fmu_pwm_servo.c delete mode 100644 src/drivers/boards/px4fmu/px4fmu_spi.c delete mode 100644 src/drivers/boards/px4fmu/px4fmu_usb.c delete mode 100644 src/drivers/boards/px4fmuv2/module.mk delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_init.c delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu2_led.c delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_can.c delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_internal.h delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_spi.c delete mode 100644 src/drivers/boards/px4fmuv2/px4fmu_usb.c create mode 100644 src/drivers/boards/px4io-v1/board_config.h create mode 100644 src/drivers/boards/px4io-v1/module.mk create mode 100644 src/drivers/boards/px4io-v1/px4io_init.c create mode 100644 src/drivers/boards/px4io-v1/px4io_pwm_servo.c create mode 100644 src/drivers/boards/px4io-v2/board_config.h create mode 100644 src/drivers/boards/px4io-v2/module.mk create mode 100644 src/drivers/boards/px4io-v2/px4iov2_init.c create mode 100644 src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c delete mode 100644 src/drivers/boards/px4io/module.mk delete mode 100644 src/drivers/boards/px4io/px4io_init.c delete mode 100644 src/drivers/boards/px4io/px4io_internal.h delete mode 100644 src/drivers/boards/px4io/px4io_pwm_servo.c delete mode 100644 src/drivers/boards/px4iov2/module.mk delete mode 100644 src/drivers/boards/px4iov2/px4iov2_init.c delete mode 100755 src/drivers/boards/px4iov2/px4iov2_internal.h delete mode 100644 src/drivers/boards/px4iov2/px4iov2_pwm_servo.c diff --git a/Makefile b/Makefile index a703bef4c..6f58858f0 100644 --- a/Makefile +++ b/Makefile @@ -148,7 +148,7 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && ln -sf $(PX4_BASE)/nuttx-configs/* .) + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/* .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e6ec840f9..255093e67 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -17,7 +17,7 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/px4io MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu +MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 MODULES += drivers/bma180 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 4add744d0..75573e2c2 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -17,7 +17,7 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/px4fmu MODULES += drivers/px4io -MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/rgbled MODULES += drivers/lsm303d MODULES += drivers/l3gd20 @@ -29,12 +29,15 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm -MODULES += drivers/mkblctrl MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += modules/sensors +# Needs to be burned to the ground and re-written; for now, +# just don't build it. +#MODULES += drivers/mkblctrl + # # System commands # diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 98fd6feda..0f60e88b5 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -13,7 +13,7 @@ ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test MODULES += drivers/device MODULES += drivers/stm32 MODULES += drivers/led -MODULES += drivers/boards/px4fmuv2 +MODULES += drivers/boards/px4fmu-v2 MODULES += systemcmds/perf MODULES += systemcmds/reboot diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk index cf70391bc..73f8adf20 100644 --- a/makefiles/config_px4io-v1_default.mk +++ b/makefiles/config_px4io-v1_default.mk @@ -6,5 +6,5 @@ # Board support modules # MODULES += drivers/stm32 -MODULES += drivers/boards/px4io +MODULES += drivers/boards/px4io-v1 MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk index f9f35d629..dbeaba3d3 100644 --- a/makefiles/config_px4io-v2_default.mk +++ b/makefiles/config_px4io-v2_default.mk @@ -6,5 +6,5 @@ # Board support modules # MODULES += drivers/stm32 -MODULES += drivers/boards/px4iov2 +MODULES += drivers/boards/px4io-v2 MODULES += modules/px4iofirmware \ No newline at end of file diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 3ad13088b..2085d45dd 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -176,6 +176,11 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # EXTRA_CLEANS = +# +# Append the per-board driver directory to the header search path. +# +INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD) + ################################################################################ # NuttX libraries and paths ################################################################################ diff --git a/makefiles/setup.mk b/makefiles/setup.mk index a0e880a0d..fdb161201 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -64,6 +64,7 @@ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ export MKFW = $(PX4_BASE)/Tools/px_mkfw.py export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py export COPY = cp +export COPYDIR = cp -Rf export REMOVE = rm -f export RMDIR = rm -rf export GENROMFS = genromfs diff --git a/nuttx-configs/px4fmu-v1/Kconfig b/nuttx-configs/px4fmu-v1/Kconfig new file mode 100644 index 000000000..edbafa06f --- /dev/null +++ b/nuttx-configs/px4fmu-v1/Kconfig @@ -0,0 +1,21 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMU_V1 + +config HRT_TIMER + bool "High resolution timer support" + default y + ---help--- + Enable high resolution timer for PPM capture and system clocks. + +config HRT_PPM + bool "PPM input capture" + default y + depends on HRT_TIMER + ---help--- + Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) + +endif diff --git a/nuttx-configs/px4fmu-v1/common/Make.defs b/nuttx-configs/px4fmu-v1/common/Make.defs new file mode 100644 index 000000000..756286ccb --- /dev/null +++ b/nuttx-configs/px4fmu-v1/common/Make.defs @@ -0,0 +1,184 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v1/common/ld.script b/nuttx-configs/px4fmu-v1/common/ld.script new file mode 100644 index 000000000..de8179e8d --- /dev/null +++ b/nuttx-configs/px4fmu-v1/common/ld.script @@ -0,0 +1,149 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F405 has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h new file mode 100644 index 000000000..1921f7715 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -0,0 +1,319 @@ +/************************************************************************************ + * configs/stm32f4discovery/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************************/ + +#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMU uses a 24MHz crystal connected to the HSE. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* LED definitions ******************************************************************/ +/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with stm32_setled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * px4fmu-v1. The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + * + * Note that UART5 has no optional pinout, so it is not listed here. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 +#define GPIO_USART2_RTS GPIO_USART2_RTS_1 +#define GPIO_USART2_CTS GPIO_USART2_CTS_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* UART DMA configuration for USART1/6 */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_2 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) + +/* + * SPI + * + * There are sensors on SPI1, and SPI3 is connected to the microSD slot. + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 +#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 + +/* SPI DMA configuration for SPI3 (microSD) */ +#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 +#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2 +/* XXX since we allocate the HP work stack from CCM RAM on normal system startup, + SPI1 will never run in DMA mode - so we can just give it a random config here. + What we really need to do is to make DMA configurable per channel, and always + disable it for SPI1. */ +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +/************************************************************************************ + * Name: stm32_ledinit, stm32_setled, and stm32_setleds + * + * Description: + * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If + * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to + * control the LEDs from user applications. + * + ************************************************************************************/ + +#ifndef CONFIG_ARCH_LEDS +EXTERN void stm32_ledinit(void); +EXTERN void stm32_setled(int led, bool ledon); +EXTERN void stm32_setleds(uint8_t ledset); +#endif + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs new file mode 100644 index 000000000..81936334b --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu-v1/common/Make.defs diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig new file mode 100644 index 000000000..eb225e22c --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -0,0 +1,957 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM3U is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +CONFIG_ARCH_CHIP_STM32F405RG=y +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=y +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +# CONFIG_STM32_SPI2 is not set +CONFIG_STM32_SPI3=y +CONFIG_STM32_SYSCFG=y +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_TIM6=y +CONFIG_STM32_TIM7=y +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM12=y +CONFIG_STM32_TIM13=y +CONFIG_STM32_TIM14=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +CONFIG_STM32_UART5=y +CONFIG_STM32_USART6=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +CONFIG_STM32_JTAG_FULL_ENABLE=y +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +# CONFIG_STM32_JTAG_SW_ENABLE is not set +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM12_PWM is not set +# CONFIG_STM32_TIM13_PWM is not set +# CONFIG_STM32_TIM14_PWM is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RXDMA is not set +# CONFIG_UART4_RXDMA is not set +# CONFIG_UART5_RS485 is not set +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y +# CONFIG_USART7_RXDMA is not set +# CONFIG_USART8_RXDMA is not set +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4FMU_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4fmu-v1" + +# +# Common Board Options +# +CONFIG_ARCH_HAVE_LEDS=y +# CONFIG_ARCH_LEDS is not set +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDSPIPORTNO=3 + +# +# Board-Specific Options +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +# CONFIG_DEV_CONSOLE is not set +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +# CONFIG_DISABLE_ENVIRON is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SPICLOCK=24000000 +# CONFIG_MMCSD_SDIO is not set +# CONFIG_MTD is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART5=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_UART5_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_NO_SERIAL_CONSOLE=y + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 + +# +# UART5 Configuration +# +CONFIG_UART5_RXBUFSIZE=32 +CONFIG_UART5_TXBUFSIZE=32 +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_BITS=8 +CONFIG_UART5_PARITY=0 +CONFIG_UART5_2STOP=0 + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +CONFIG_USBDEV_SELFPOWERED=y +# CONFIG_USBDEV_BUSPOWERED is not set +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG=y +CONFIG_SYSLOG_CHAR=y +CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +CONFIG_GRAN_SINGLE=y +CONFIG_GRAN_INTR=y + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +# CONFIG_C99_BOOL8 is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/px4fmu-v1/nsh/setenv.sh b/nuttx-configs/px4fmu-v1/nsh/setenv.sh new file mode 100755 index 000000000..db372217c --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# configs/px4fmu-v1/usbnsh/setenv.sh +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This is the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/ostest/Make.defs b/nuttx-configs/px4fmu-v1/ostest/Make.defs new file mode 100644 index 000000000..7b807abdb --- /dev/null +++ b/nuttx-configs/px4fmu-v1/ostest/Make.defs @@ -0,0 +1,122 @@ +############################################################################ +# configs/stm32f4discovery/ostest/Make.defs +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" + MAXOPTIMIZATION = -O2 +else + # Linux/Cygwin-native toolchain + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) +endif + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(ARCROSSDEV)ar rcs +NM = $(ARCROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + ARCHOPTIMIZATION = -g +else + ARCHOPTIMIZATION = $(MAXOPTIMIZATION) -fno-strict-aliasing -fno-strength-reduce -fomit-frame-pointer +endif + +ARCHCFLAGS = -fno-builtin +ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti +ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow +ARCHWARNINGSXX = -Wall -Wshadow +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +CFLAGS = $(ARCHCFLAGS) $(ARCHWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-pcrel.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +ifneq ($(CROSSDEV),arm-nuttx-elf-) + LDFLAGS += -nostartfiles -nodefaultlibs +endif +ifeq ($(CONFIG_DEBUG_SYMBOLS),y) + LDFLAGS += -g +endif + + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = +ifeq ($(CONFIG_HOST_WINDOWS),y) + HOSTEXEEXT = .exe +else + HOSTEXEEXT = +endif + +ifeq ($(WINTOOL),y) + # Windows-native host tools + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh +else + # Linux/Cygwin-native host tools + MKDEP = $(TOPDIR)/tools/mkdeps$(HOSTEXEEXT) +endif + diff --git a/nuttx-configs/px4fmu-v1/ostest/defconfig b/nuttx-configs/px4fmu-v1/ostest/defconfig new file mode 100644 index 000000000..c7fb6b2a5 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/ostest/defconfig @@ -0,0 +1,583 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +# CONFIG_HOST_OSX is not set +CONFIG_HOST_WINDOWS=y +# CONFIG_HOST_OTHER is not set +# CONFIG_WINDOWS_NATIVE is not set +CONFIG_WINDOWS_CYGWIN=y +# CONFIG_WINDOWS_MSYS is not set +# CONFIG_WINDOWS_OTHER is not set +# CONFIG_WINDOWS_MKLINK is not set + +# +# Build Configuration +# +# CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +CONFIG_INTELHEX_BINARY=y +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +# CONFIG_ARCH_MATH_H is not set +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +# CONFIG_DEBUG_SYMBOLS is not set + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_LM3S is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_SAM3U is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_HAVE_CMNVECTOR=y +# CONFIG_ARMV7M_CMNVECTOR is not set +# CONFIG_ARCH_FPU is not set +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set +CONFIG_ARCH_IRQPRIO=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +# CONFIG_SERIAL_TERMIOS is not set + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +CONFIG_ARCH_CHIP_STM32F407VG=y +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_CODESOURCERYW is not set +CONFIG_STM32_CODESOURCERYL=y +# CONFIG_STM32_ATOLLIC_LITE is not set +# CONFIG_STM32_ATOLLIC_PRO is not set +# CONFIG_STM32_DEVKITARM is not set +# CONFIG_STM32_RAISONANCE is not set +# CONFIG_STM32_BUILDROOT is not set +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +# CONFIG_STM32_ADC1 is not set +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +# CONFIG_STM32_BKPSRAM is not set +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +# CONFIG_STM32_CCMDATARAM is not set +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +# CONFIG_STM32_DMA1 is not set +# CONFIG_STM32_DMA2 is not set +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +# CONFIG_STM32_I2C1 is not set +# CONFIG_STM32_I2C2 is not set +# CONFIG_STM32_I2C3 is not set +# CONFIG_STM32_IWDG is not set +# CONFIG_STM32_OTGFS is not set +# CONFIG_STM32_OTGHS is not set +# CONFIG_STM32_PWR is not set +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +# CONFIG_STM32_SPI1 is not set +# CONFIG_STM32_SPI2 is not set +# CONFIG_STM32_SPI3 is not set +CONFIG_STM32_SYSCFG=y +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +# CONFIG_STM32_TIM4 is not set +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +# CONFIG_STM32_TIM9 is not set +# CONFIG_STM32_TIM10 is not set +# CONFIG_STM32_TIM11 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +# CONFIG_STM32_USART1 is not set +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +# CONFIG_STM32_WWDG is not set + +# +# Alternate Pin Mapping +# +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set + +# +# USB Host Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_DMA is not set +CONFIG_ARCH_STACKDUMP=y + +# +# Board Settings +# +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=114688 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=0 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="stm32f4discovery" + +# +# Common Board Options +# +CONFIG_ARCH_HAVE_LEDS=y +CONFIG_ARCH_LEDS=y +CONFIG_ARCH_HAVE_BUTTONS=y +# CONFIG_ARCH_BUTTONS is not set +CONFIG_ARCH_HAVE_IRQBUTTONS=y + +# +# Board-Specific Options +# + +# +# RTOS Features +# +CONFIG_MSEC_PER_TICK=10 +CONFIG_RR_INTERVAL=200 +# CONFIG_SCHED_INSTRUMENTATION is not set +CONFIG_TASK_NAME_SIZE=0 +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=2009 +CONFIG_START_MONTH=9 +CONFIG_START_DAY=21 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +# CONFIG_PRIORITY_INHERITANCE is not set +# CONFIG_FDCLONE_DISABLE is not set +# CONFIG_FDCLONE_STDIO is not set +CONFIG_SDCLONE_DISABLE=y +# CONFIG_SCHED_WORKQUEUE is not set +# CONFIG_SCHED_WAITPID is not set +# CONFIG_SCHED_ATEXIT is not set +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="ostest_main" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=16 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=4 + +# +# Stack and heap information +# +# CONFIG_CUSTOM_STACK is not set +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=2048 +CONFIG_PTHREAD_STACK_MIN=256 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +# CONFIG_I2C is not set +CONFIG_ARCH_HAVE_I2CRESET=y +# CONFIG_SPI is not set +# CONFIG_RTC is not set +# CONFIG_WATCHDOG is not set +# CONFIG_ANALOG is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +# CONFIG_MTD is not set +# CONFIG_PIPES is not set +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +# CONFIG_SERCOMM_CONSOLE is not set +CONFIG_SERIAL=y +CONFIG_DEV_LOWCONSOLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_USART2=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=128 +CONFIG_USART2_TXBUFSIZE=128 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_FS_RAMMAP is not set + +# +# System Logging +# +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +# CONFIG_GRAN is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_SYMTAB_ORDEREDBYNAME=y + +# +# Library Routines +# +CONFIG_STDIO_BUFFER_SIZE=64 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +# CONFIG_LIBM is not set +# CONFIG_NOPRINTF_FIELDWIDTH is not set +# CONFIG_LIBC_FLOATINGPOINT is not set +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_STRERROR is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set + +# +# Basic CXX Support +# +# CONFIG_HAVE_CXX is not set + +# +# Application Configuration +# + +# +# Named Applications +# +# CONFIG_BUILTIN is not set + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_CDCACM is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MOUNT is not set +# CONFIG_EXAMPLES_MODBUS is not set +# CONFIG_EXAMPLES_NETTEST is not set +# CONFIG_EXAMPLES_NSH is not set +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +CONFIG_EXAMPLES_OSTEST=y +# CONFIG_EXAMPLES_OSTEST_BUILTIN is not set +CONFIG_EXAMPLES_OSTEST_LOOPS=1 +CONFIG_EXAMPLES_OSTEST_STACKSIZE=2048 +CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS=3 +CONFIG_EXAMPLES_OSTEST_RR_RANGE=10000 +CONFIG_EXAMPLES_OSTEST_RR_RUNS=10 +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POLL is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set +# CONFIG_EXAMPLES_WLAN is not set + +# +# Interpreters +# + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# ModBus +# + +# +# FreeModbus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# readline() +# +# CONFIG_SYSTEM_READLINE is not set + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +# CONFIG_SYSTEM_SYSINFO is not set diff --git a/nuttx-configs/px4fmu-v1/ostest/setenv.sh b/nuttx-configs/px4fmu-v1/ostest/setenv.sh new file mode 100755 index 000000000..a67fdc5a8 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/ostest/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# configs/stm32f4discovery/ostest/setenv.sh +# +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This is the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld new file mode 100644 index 000000000..1f29f02f5 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm32f4discovery/scripts/gnu-elf.ld + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +SECTIONS +{ + .text 0x00000000 : + { + _stext = . ; + *(.text) + *(.text.*) + *(.gnu.warning) + *(.stub) + *(.glue_7) + *(.glue_7t) + *(.jcr) + + /* C++ support: The .init and .fini sections contain specific logic + * to manage static constructors and destructors. + */ + + *(.gnu.linkonce.t.*) + *(.init) /* Old ABI */ + *(.fini) /* Old ABI */ + _etext = . ; + } + + .rodata : + { + _srodata = . ; + *(.rodata) + *(.rodata1) + *(.rodata.*) + *(.gnu.linkonce.r*) + _erodata = . ; + } + + .data : + { + _sdata = . ; + *(.data) + *(.data1) + *(.data.*) + *(.gnu.linkonce.d*) + _edata = . ; + } + + /* C++ support. For each global and static local C++ object, + * GCC creates a small subroutine to construct the object. Pointers + * to these routines (not the routines themselves) are stored as + * simple, linear arrays in the .ctors section of the object file. + * Similarly, pointers to global/static destructor routines are + * stored in .dtors. + */ + + .ctors : + { + _sctors = . ; + *(.ctors) /* Old ABI: Unallocated */ + *(.init_array) /* New ABI: Allocated */ + _edtors = . ; + } + + .dtors : + { + _sdtors = . ; + *(.dtors) /* Old ABI: Unallocated */ + *(.fini_array) /* New ABI: Allocated */ + _edtors = . ; + } + + .bss : + { + _sbss = . ; + *(.bss) + *(.bss.*) + *(.sbss) + *(.sbss.*) + *(.gnu.linkonce.b*) + *(COMMON) + _ebss = . ; + } + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script new file mode 100644 index 000000000..f6560743b --- /dev/null +++ b/nuttx-configs/px4fmu-v1/scripts/ld.script @@ -0,0 +1,123 @@ +/**************************************************************************** + * configs/stm32f4discovery/scripts/ld.script + * + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 112K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + .init_section : { + _sinit = ABSOLUTE(.); + *(.init_array .init_array.*) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/src/Makefile b/nuttx-configs/px4fmu-v1/src/Makefile new file mode 100644 index 000000000..6ef8b7d6a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu-v1/src/Makefile +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py new file mode 100755 index 000000000..9f4ddad62 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 firmware image generator +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# + +import sys +import argparse +import json +import base64 +import zlib +import time +import subprocess + +# +# Construct a basic firmware description +# +def mkdesc(): + proto = {} + proto['magic'] = "PX4FWv1" + proto['board_id'] = 0 + proto['board_revision'] = 0 + proto['version'] = "" + proto['summary'] = "" + proto['description'] = "" + proto['git_identity'] = "" + proto['build_time'] = 0 + proto['image'] = base64.b64encode(bytearray()) + proto['image_size'] = 0 + return proto + +# Parse commandline +parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.") +parser.add_argument("--prototype", action="store", help="read a prototype description from a file") +parser.add_argument("--board_id", action="store", help="set the board ID required") +parser.add_argument("--board_revision", action="store", help="set the board revision required") +parser.add_argument("--version", action="store", help="set a version string") +parser.add_argument("--summary", action="store", help="set a brief description") +parser.add_argument("--description", action="store", help="set a longer description") +parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--image", action="store", help="the firmware image") +args = parser.parse_args() + +# Fetch the firmware descriptor prototype if specified +if args.prototype != None: + f = open(args.prototype,"r") + desc = json.load(f) + f.close() +else: + desc = mkdesc() + +desc['build_time'] = int(time.time()) + +if args.board_id != None: + desc['board_id'] = int(args.board_id) +if args.board_revision != None: + desc['board_revision'] = int(args.board_revision) +if args.version != None: + desc['version'] = str(args.version) +if args.summary != None: + desc['summary'] = str(args.summary) +if args.description != None: + desc['description'] = str(args.description) +if args.git_identity != None: + cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) + p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout + desc['git_identity'] = p.read().strip() + p.close() +if args.image != None: + f = open(args.image, "rb") + bytes = f.read() + desc['image_size'] = len(bytes) + desc['image'] = base64.b64encode(zlib.compress(bytes,9)) + +print json.dumps(desc, indent=4) diff --git a/nuttx-configs/px4fmu-v1/tools/px_uploader.py b/nuttx-configs/px4fmu-v1/tools/px_uploader.py new file mode 100755 index 000000000..3b23f4f83 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/tools/px_uploader.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# Serial firmware uploader for the PX4FMU bootloader +# +# The PX4 firmware file is a JSON-encoded Python object, containing +# metadata fields and a zlib-compressed base64-encoded firmware image. +# +# The uploader uses the following fields from the firmware file: +# +# image +# The firmware that will be uploaded. +# image_size +# The size of the firmware in bytes. +# board_id +# The board for which the firmware is intended. +# board_revision +# Currently only used for informational purposes. +# + +import sys +import argparse +import binascii +import serial +import os +import struct +import json +import zlib +import base64 +import time +import array + +from sys import platform as _platform + +class firmware(object): + '''Loads a firmware file''' + + desc = {} + image = bytes() + crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ]) + crcpad = bytearray('\xff\xff\xff\xff') + + def __init__(self, path): + + # read the file + f = open(path, "r") + self.desc = json.load(f) + f.close() + + self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) + + # pad image to 4-byte length + while ((len(self.image) % 4) != 0): + self.image.append('\xff') + + def property(self, propname): + return self.desc[propname] + + def __crc32(self, bytes, state): + for byte in bytes: + index = (state ^ byte) & 0xff + state = self.crctab[index] ^ (state >> 8) + return state + + def crc(self, padlen): + state = self.__crc32(self.image, int(0)) + for i in range(len(self.image), (padlen - 1), 4): + state = self.__crc32(self.crcpad, state) + return state + +class uploader(object): + '''Uploads a firmware file to the PX FMU bootloader''' + + # protocol bytes + INSYNC = chr(0x12) + EOC = chr(0x20) + + # reply bytes + OK = chr(0x10) + FAILED = chr(0x11) + INVALID = chr(0x13) # rev3+ + + # command bytes + NOP = chr(0x00) # guaranteed to be discarded by the bootloader + GET_SYNC = chr(0x21) + GET_DEVICE = chr(0x22) + CHIP_ERASE = chr(0x23) + CHIP_VERIFY = chr(0x24) # rev2 only + PROG_MULTI = chr(0x27) + READ_MULTI = chr(0x28) # rev2 only + GET_CRC = chr(0x29) # rev3+ + REBOOT = chr(0x30) + + INFO_BL_REV = chr(1) # bootloader protocol revision + BL_REV_MIN = 2 # minimum supported bootloader protocol + BL_REV_MAX = 3 # maximum supported bootloader protocol + INFO_BOARD_ID = chr(2) # board type + INFO_BOARD_REV = chr(3) # board revision + INFO_FLASH_SIZE = chr(4) # max firmware size in bytes + + PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 + READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + + def __init__(self, portname, baudrate): + # open the port, keep the default timeout short so we can poll quickly + self.port = serial.Serial(portname, baudrate, timeout=0.25) + + def close(self): + if self.port is not None: + self.port.close() + + def __send(self, c): +# print("send " + binascii.hexlify(c)) + self.port.write(str(c)) + + def __recv(self, count = 1): + c = self.port.read(count) + if len(c) < 1: + raise RuntimeError("timeout waiting for data") +# print("recv " + binascii.hexlify(c)) + return c + + def __recv_int(self): + raw = self.__recv(4) + val = struct.unpack("= 3: + self.__getSync() + + # split a sequence into a list of size-constrained pieces + def __split_len(self, seq, length): + return [seq[i:i+length] for i in range(0, len(seq), length)] + + # upload code + def __program(self, fw): + code = fw.image + groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + for bytes in groups: + self.__program_multi(bytes) + + # verify code + def __verify_v2(self, fw): + self.__send(uploader.CHIP_VERIFY + + uploader.EOC) + self.__getSync() + code = fw.image + groups = self.__split_len(code, uploader.READ_MULTI_MAX) + for bytes in groups: + if (not self.__verify_multi(bytes)): + raise RuntimeError("Verification failed") + + def __verify_v3(self, fw): + expect_crc = fw.crc(self.fw_maxsize) + self.__send(uploader.GET_CRC + + uploader.EOC) + report_crc = self.__recv_int() + self.__getSync() + if report_crc != expect_crc: + print("Expected 0x%x" % expect_crc) + print("Got 0x%x" % report_crc) + raise RuntimeError("Program CRC failed") + + # get basic data about the board + def identify(self): + # make sure we are in sync before starting + self.__sync() + + # get the bootloader protocol ID first + self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) + if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): + print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) + raise RuntimeError("Bootloader protocol mismatch") + + self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) + self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) + self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + + # upload the firmware + def upload(self, fw): + # Make sure we are doing the right thing + if self.board_type != fw.property('board_id'): + raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).") + if self.fw_maxsize < fw.property('image_size'): + raise RuntimeError("Firmware image is too large for this board") + + print("erase...") + self.__erase() + + print("program...") + self.__program(fw) + + print("verify...") + if self.bl_rev == 2: + self.__verify_v2(fw) + else: + self.__verify_v3(fw) + + print("done, rebooting.") + self.__reboot() + self.port.close() + + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") +parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") +parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") +args = parser.parse_args() + +# Load the firmware file +fw = firmware(args.firmware) +print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) + +# Spin waiting for a device to show up +while True: + for port in args.port.split(","): + + #print("Trying %s" % port) + + # create an uploader attached to the port + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) + except: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + # port is open, try talking to it + try: + # identify the bootloader + up.identify() + print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + + except: + # most probably a timeout talking to the port, no bootloader + continue + + try: + # ok, we have a bootloader, try flashing it + up.upload(fw) + + except RuntimeError as ex: + + # print the error + print("ERROR: %s" % ex.args) + + finally: + # always close the port + up.close() + + # we could loop here if we wanted to wait for more boards... + sys.exit(0) diff --git a/nuttx-configs/px4fmu-v1/tools/upload.sh b/nuttx-configs/px4fmu-v1/tools/upload.sh new file mode 100755 index 000000000..4e6597b3a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/tools/upload.sh @@ -0,0 +1,27 @@ +#!/bin/sh +# +# Wrapper to upload a PX4 firmware binary +# +TOOLS=`dirname $0` +MKFW=${TOOLS}/px_mkfw.py +UPLOAD=${TOOLS}/px_uploader.py + +BINARY=nuttx.bin +PAYLOAD=nuttx.px4 +PORTS="/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" + +function abort() { + echo "ABORT: $*" + exit 1 +} + +if [ ! -f ${MKFW} -o ! -f ${UPLOAD} ]; then + abort "Missing tools ${MKFW} and/or ${UPLOAD}" +fi +if [ ! -f ${BINARY} ]; then + abort "Missing nuttx binary in current directory." +fi + +rm -f ${PAYLOAD} +${MKFW} --board_id 5 --image ${BINARY} > ${PAYLOAD} +${UPLOAD} --port ${PORTS} ${PAYLOAD} diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index a6cdfb4d2..507df70a2 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -194,11 +194,6 @@ #define DMAMAP_SDIO DMAMAP_SDIO_1 -/* High-resolution timer - */ -#define HRT_TIMER 8 /* use timer8 for the HRT */ -#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ - /* Alternate function pin selections ************************************************/ /* @@ -232,35 +227,12 @@ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* - * PWM - * - * Six PWM outputs are configured. - * - * Pins: - * - * CH1 : PE14 : TIM1_CH4 - * CH2 : PE13 : TIM1_CH3 - * CH3 : PE11 : TIM1_CH2 - * CH4 : PE9 : TIM1_CH1 - * CH5 : PD13 : TIM4_CH2 - * CH6 : PD14 : TIM4_CH3 - * - */ -#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 -#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 -#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 -#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 -#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 -#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 - /* * CAN * * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ - #define GPIO_CAN1_RX GPIO_CAN1_RX_3 #define GPIO_CAN1_TX GPIO_CAN1_TX_3 #define GPIO_CAN2_RX GPIO_CAN2_RX_1 @@ -283,20 +255,6 @@ #define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) #define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) -/* - * I2C busses - */ -#define PX4_I2C_BUS_EXPANSION 1 -#define PX4_I2C_BUS_LED 2 - -/* - * Devices on the onboard bus. - * - * Note that these are unshifted addresses. - */ -#define PX4_I2C_OBDEV_LED 0x55 -#define PX4_I2C_OBDEV_HMC5883 0x1e - /* * SPI * @@ -310,22 +268,6 @@ #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 #define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 -/* - * Use these in place of the spi_dev_e enumeration to - * select a specific SPI device on SPI1 - */ -#define PX4_SPIDEV_GYRO 1 -#define PX4_SPIDEV_ACCEL_MAG 2 -#define PX4_SPIDEV_BARO 3 - -/* - * Tone alarm output - */ -#define TONE_ALARM_TIMER 2 /* timer 2 */ -#define TONE_ALARM_CHANNEL 1 /* channel 1 */ -#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) -#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) - /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/nuttx-configs/px4io-v1/Kconfig b/nuttx-configs/px4io-v1/Kconfig new file mode 100644 index 000000000..fbf74d7f0 --- /dev/null +++ b/nuttx-configs/px4io-v1/Kconfig @@ -0,0 +1,21 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4IO_V1 + +config HRT_TIMER + bool "High resolution timer support" + default y + ---help--- + Enable high resolution timer for PPM capture and system clocks. + +config HRT_PPM + bool "PPM input capture" + default y + depends on HRT_TIMER + ---help--- + Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) + +endif diff --git a/nuttx-configs/px4io-v1/README.txt b/nuttx-configs/px4io-v1/README.txt new file mode 100755 index 000000000..9b1615f42 --- /dev/null +++ b/nuttx-configs/px4io-v1/README.txt @@ -0,0 +1,806 @@ +README +====== + +This README discusses issues unique to NuttX configurations for the +STMicro STM3210E-EVAL development board. + +Contents +======== + + - Development Environment + - GNU Toolchain Options + - IDEs + - NuttX buildroot Toolchain + - DFU and JTAG + - OpenOCD + - LEDs + - Temperature Sensor + - RTC + - STM3210E-EVAL-specific Configuration Options + - Configurations + +Development Environment +======================= + + Either Linux or Cygwin on Windows can be used for the development environment. + The source has been built only using the GNU toolchain (see below). Other + toolchains will likely cause problems. Testing was performed using the Cygwin + environment because the Raisonance R-Link emulatator and some RIDE7 development tools + were used and those tools works only under Windows. + +GNU Toolchain Options +===================== + + The NuttX make system has been modified to support the following different + toolchain options. + + 1. The CodeSourcery GNU toolchain, + 2. The devkitARM GNU toolchain, + 3. Raisonance GNU toolchain, or + 4. The NuttX buildroot Toolchain (see below). + + All testing has been conducted using the NuttX buildroot toolchain. However, + the make system is setup to default to use the devkitARM toolchain. To use + the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to + add one of the following configuration options to your .config (or defconfig) + file: + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux + CONFIG_STM32_DEVKITARM=y : devkitARM under Windows + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) + + If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify + the PATH in the setenv.h file if your make cannot find the tools. + + NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are + Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot + toolchains are Cygwin and/or Linux native toolchains. There are several limitations + to using a Windows based toolchain in a Cygwin environment. The three biggest are: + + 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are + performed automatically in the Cygwin makefiles using the 'cygpath' utility + but you might easily find some new path problems. If so, check out 'cygpath -w' + + 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links + are used in Nuttx (e.g., include/arch). The make system works around these + problems for the Windows tools by copying directories instead of linking them. + But this can also cause some confusion for you: For example, you may edit + a file in a "linked" directory and find that your changes had no effect. + That is because you are building the copy of the file in the "fake" symbolic + directory. If you use a Windows toolchain, you should get in the habit of + making like this: + + make clean_context all + + An alias in your .bashrc file might make that less painful. + + 3. Dependencies are not made when using Windows versions of the GCC. This is + because the dependencies are generated using Windows pathes which do not + work with the Cygwin make. + + Support has been added for making dependencies with the windows-native toolchains. + That support can be enabled by modifying your Make.defs file as follows: + + - MKDEP = $(TOPDIR)/tools/mknulldeps.sh + + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" + + If you have problems with the dependency build (for example, if you are not + building on C:), then you may need to modify tools/mkdeps.sh + + NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization + level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with + -Os. + + NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that + the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM + path or will get the wrong version of make. + +IDEs +==== + + NuttX is built using command-line make. It can be used with an IDE, but some + effort will be required to create the project (There is a simple RIDE project + in the RIDE subdirectory). + + Makefile Build + -------------- + Under Eclipse, it is pretty easy to set up an "empty makefile project" and + simply use the NuttX makefile to build the system. That is almost for free + under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty + makefile project in order to work with Windows (Google for "Eclipse Cygwin" - + there is a lot of help on the internet). + + Native Build + ------------ + Here are a few tips before you start that effort: + + 1) Select the toolchain that you will be using in your .config file + 2) Start the NuttX build at least one time from the Cygwin command line + before trying to create your project. This is necessary to create + certain auto-generated files and directories that will be needed. + 3) Set up include pathes: You will need include/, arch/arm/src/stm32, + arch/arm/src/common, arch/arm/src/armv7-m, and sched/. + 4) All assembly files need to have the definition option -D __ASSEMBLY__ + on the command line. + + Startup files will probably cause you some headaches. The NuttX startup file + is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX + one time from the Cygwin command line in order to obtain the pre-built + startup object needed by RIDE. + +NuttX buildroot Toolchain +========================= + + A GNU GCC-based toolchain is assumed. The files */setenv.sh should + be modified to point to the correct path to the Cortex-M3 GCC toolchain (if + different from the default in your PATH variable). + + If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX + SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). + This GNU toolchain builds and executes in the Linux or Cygwin environment. + + 1. You must have already configured Nuttx in /nuttx. + + cd tools + ./configure.sh stm3210e-eval/ + + 2. Download the latest buildroot package into + + 3. unpack the buildroot tarball. The resulting directory may + have versioning information on it like buildroot-x.y.z. If so, + rename /buildroot-x.y.z to /buildroot. + + 4. cd /buildroot + + 5. cp configs/cortexm3-defconfig-4.3.3 .config + + 6. make oldconfig + + 7. make + + 8. Edit setenv.h, if necessary, so that the PATH variable includes + the path to the newly built binaries. + + See the file configs/README.txt in the buildroot source tree. That has more + detailed PLUS some special instructions that you will need to follow if you are + building a Cortex-M3 toolchain for Cygwin under Windows. + +DFU and JTAG +============ + + Enbling Support for the DFU Bootloader + -------------------------------------- + The linker files in these projects can be configured to indicate that you + will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) + loader or via some JTAG emulator. You can specify the DFU bootloader by + adding the following line: + + CONFIG_STM32_DFU=y + + to your .config file. Most of the configurations in this directory are set + up to use the DFU loader. + + If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning + of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed + to make space for the DFU loader and 0x08003000 is where the DFU loader expects + to find new applications at boot time. If you need to change that origin for some + other bootloader, you will need to edit the file(s) ld.script.dfu for each + configuration. + + The DFU SE PC-based software is available from the STMicro website, + http://www.st.com. General usage instructions: + + 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU + file (nuttx.dfu)... see below for details. + 2. Connect the STM3210E-EVAL board to your computer using a USB + cable. + 3. Start the DFU loader on the STM3210E-EVAL board. You do this by + resetting the board while holding the "Key" button. Windows should + recognize that the DFU loader has been installed. + 3. Run the DFU SE program to load nuttx.dfu into FLASH. + + What if the DFU loader is not in FLASH? The loader code is available + inside of the Demo dirctory of the USBLib ZIP file that can be downloaded + from the STMicro Website. You can build it using RIDE (or other toolchains); + you will need a JTAG emulator to burn it into FLASH the first time. + + In order to use STMicro's built-in DFU loader, you will have to get + the NuttX binary into a special format with a .dfu extension. The + DFU SE PC_based software installation includes a file "DFU File Manager" + conversion program that a file in Intel Hex format to the special DFU + format. When you successfully build NuttX, you will find a file called + nutt.ihx in the top-level directory. That is the file that you should + provide to the DFU File Manager. You will need to rename it to nuttx.hex + in order to find it with the DFU File Manager. You will end up with + a file called nuttx.dfu that you can use with the STMicro DFU SE program. + + Enabling JTAG + ------------- + If you are not using the DFU, then you will probably also need to enable + JTAG support. By default, all JTAG support is disabled but there NuttX + configuration options to enable JTAG in various different ways. + + These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO + MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the + Cortex debug port. The default state in this port is for all JTAG support + to be disable. + + CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full + SWJ (JTAG-DP + SW-DP) + + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable + full SWJ (JTAG-DP + SW-DP) but without JNTRST. + + CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP + disabled and SW-DP enabled + + The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 + which disable JTAG-DP and SW-DP. + +OpenOCD +======= + +I have also used OpenOCD with the STM3210E-EVAL. In this case, I used +the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh +for more information. Using the script: + +1) Start the OpenOCD GDB server + + cd + configs/stm3210e-eval/tools/oocd.sh $PWD + +2) Load Nuttx + + cd + arm-none-eabi-gdb nuttx + gdb> target remote localhost:3333 + gdb> mon reset + gdb> mon halt + gdb> load nuttx + +3) Running NuttX + + gdb> mon reset + gdb> c + +LEDs +==== + +The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the +board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is +defined. In that case, the usage by the board port is defined in +include/board.h and src/up_leds.c. The LEDs are used to encode OS-related +events as follows: + + SYMBOL Meaning LED1* LED2 LED3 LED4 + ---------------- ----------------------- ----- ----- ----- ----- + LED_STARTED NuttX has been started ON OFF OFF OFF + LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF + LED_IRQSENABLED Interrupts enabled ON ON OFF OFF + LED_STACKCREATED Idle stack created OFF OFF ON OFF + LED_INIRQ In an interrupt** ON N/C N/C OFF + LED_SIGNAL In a signal handler*** N/C ON N/C OFF + LED_ASSERTION An assertion failed ON ON N/C OFF + LED_PANIC The system has crashed N/C N/C N/C ON + LED_IDLE STM32 is is sleep mode (Optional, not used) + + * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot + and these LEDs will give you some indication of where the failure was + ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + is because of timer interupts that result in the LED being illuminated + on a small proportion of the time. +*** LED2 may also flicker normally if signals are processed. + +Temperature Sensor +================== + +Support for the on-board LM-75 temperature sensor is available. This supported +has been verified, but has not been included in any of the available the +configurations. To set up the temperature sensor, add the following to the +NuttX configuration file + + CONFIG_I2C=y + CONFIG_I2C_LM75=y + +Then you can implement logic like the following to use the temperature sensor: + + #include + #include + + ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ + fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ + ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ + bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ + +More complex temperature sensor operations are also available. See the IOCTAL +commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions +of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the +arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). + +RTC +=== + + The STM32 RTC may configured using the following settings. + + CONFIG_RTC - Enables general support for a hardware RTC. Specific + architectures may require other specific settings. + CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 + second, usually supporting a 32-bit time_t value. In this case, + the RTC is used to "seed" the normal NuttX timer and the + NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES + is enabled in the NuttX configuration, then the RTC provides higher + resolution time and completely replaces the system timer for purpose of + date and time. + CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the + frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES + is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. + CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. + A callback function will be executed when the alarm goes off + + In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts + are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. + A BKP register is incremented on each overflow interrupt creating, effectively, + a 48-bit RTC counter. + + In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled + (because the next overflow is not expected until the year 2106. + + WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The + overflow interrupt may be lost even if the STM32 is powered down only momentarily. + Therefore hi-res solution is only useful in systems where the power is always on. + +STM3210E-EVAL-specific Configuration Options +============================================ + + CONFIG_ARCH - Identifies the arch/ subdirectory. This should + be set to: + + CONFIG_ARCH=arm + + CONFIG_ARCH_family - For use in C code: + + CONFIG_ARCH_ARM=y + + CONFIG_ARCH_architecture - For use in C code: + + CONFIG_ARCH_CORTEXM3=y + + CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory + + CONFIG_ARCH_CHIP=stm32 + + CONFIG_ARCH_CHIP_name - For use in C code to identify the exact + chip: + + CONFIG_ARCH_CHIP_STM32F103ZET6 + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock + configuration features. + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n + + CONFIG_ARCH_BOARD - Identifies the configs subdirectory and + hence, the board that supports the particular chip or SoC. + + CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) + + CONFIG_ARCH_BOARD_name - For use in C code + + CONFIG_ARCH_BOARD_STM3210E_EVAL=y + + CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation + of delay loops + + CONFIG_ENDIAN_BIG - define if big endian (default is little + endian) + + CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): + + CONFIG_DRAM_SIZE=0x00010000 (64Kb) + + CONFIG_DRAM_START - The start address of installed DRAM + + CONFIG_DRAM_START=0x20000000 + + CONFIG_DRAM_END - Last address+1 of installed RAM + + CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) + + CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization + + CONFIG_ARCH_IRQPRIO=y + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that + have LEDs + + CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt + stack. If defined, this symbol is the size of the interrupt + stack in bytes. If not defined, the user task stacks will be + used during interrupt handling. + + CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. + + CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that + cause a 100 second delay during boot-up. This 100 second delay + serves no purpose other than it allows you to calibratre + CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure + the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until + the delay actually is 100 seconds. + + Individual subsystems can be enabled: + AHB + --- + CONFIG_STM32_DMA1 + CONFIG_STM32_DMA2 + CONFIG_STM32_CRC + CONFIG_STM32_FSMC + CONFIG_STM32_SDIO + + APB1 + ---- + CONFIG_STM32_TIM2 + CONFIG_STM32_TIM3 + CONFIG_STM32_TIM4 + CONFIG_STM32_TIM5 + CONFIG_STM32_TIM6 + CONFIG_STM32_TIM7 + CONFIG_STM32_WWDG + CONFIG_STM32_SPI2 + CONFIG_STM32_SPI4 + CONFIG_STM32_USART2 + CONFIG_STM32_USART3 + CONFIG_STM32_UART4 + CONFIG_STM32_UART5 + CONFIG_STM32_I2C1 + CONFIG_STM32_I2C2 + CONFIG_STM32_USB + CONFIG_STM32_CAN + CONFIG_STM32_BKP + CONFIG_STM32_PWR + CONFIG_STM32_DAC1 + CONFIG_STM32_DAC2 + CONFIG_STM32_USB + + APB2 + ---- + CONFIG_STM32_ADC1 + CONFIG_STM32_ADC2 + CONFIG_STM32_TIM1 + CONFIG_STM32_SPI1 + CONFIG_STM32_TIM8 + CONFIG_STM32_USART1 + CONFIG_STM32_ADC3 + + Timer and I2C devices may need to the following to force power to be applied + unconditionally at power up. (Otherwise, the device is powered when it is + initialized). + + CONFIG_STM32_FORCEPOWER + + Timer devices may be used for different purposes. One special purpose is + to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn + is defined (as above) then the following may also be defined to indicate that + the timer is intended to be used for pulsed output modulation, ADC conversion, + or DAC conversion. Note that ADC/DAC require two definition: Not only do you have + to assign the timer (n) for used by the ADC or DAC, but then you also have to + configure which ADC or DAC (m) it is assigned to. + + CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 + CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 + CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 + CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 + CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 + + For each timer that is enabled for PWM usage, we need the following additional + configuration settings: + + CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} + + NOTE: The STM32 timers are each capable of generating different signals on + each of the four channels with different duty cycles. That capability is + not supported by this driver: Only one output channel per timer. + + Alternate pin mappings (should not be used with the STM3210E-EVAL board): + + CONFIG_STM32_TIM1_FULL_REMAP + CONFIG_STM32_TIM1_PARTIAL_REMAP + CONFIG_STM32_TIM2_FULL_REMAP + CONFIG_STM32_TIM2_PARTIAL_REMAP_1 + CONFIG_STM32_TIM2_PARTIAL_REMAP_2 + CONFIG_STM32_TIM3_FULL_REMAP + CONFIG_STM32_TIM3_PARTIAL_REMAP + CONFIG_STM32_TIM4_REMAP + CONFIG_STM32_USART1_REMAP + CONFIG_STM32_USART2_REMAP + CONFIG_STM32_USART3_FULL_REMAP + CONFIG_STM32_USART3_PARTIAL_REMAP + CONFIG_STM32_SPI1_REMAP + CONFIG_STM32_SPI3_REMAP + CONFIG_STM32_I2C1_REMAP + CONFIG_STM32_CAN1_FULL_REMAP + CONFIG_STM32_CAN1_PARTIAL_REMAP + CONFIG_STM32_CAN2_REMAP + + JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): + CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + but without JNTRST. + CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled + + STM32F103Z specific device driver settings + + CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART + m (m=4,5) for the console and ttys0 (default is the USART1). + CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. + This specific the size of the receive buffer + CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before + being sent. This specific the size of the transmit buffer + CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be + CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. + CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity + CONFIG_U[S]ARTn_2STOP - Two stop bits + + CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI + support. Non-interrupt-driven, poll-waiting is recommended if the + interrupt rate would be to high in the interrupt driven case. + CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. + Cannot be used with CONFIG_STM32_SPI_INTERRUPT. + + CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO + and CONFIG_STM32_DMA2. + CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 + CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. + Default: Medium + CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: + 4-bit transfer mode. + + STM3210E-EVAL CAN Configuration + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default + Standard 11-bit IDs. + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 + CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + + STM3210E-EVAL LCD Hardware Configuration + + CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" + support. Default is this 320x240 "landscape" orientation + (this setting is informative only... not used). + CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" + orientation support. In this orientation, the STM3210E-EVAL's + LCD ribbon cable is at the bottom of the display. Default is + 320x240 "landscape" orientation. + CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse + portrait" orientation support. In this orientation, the + STM3210E-EVAL's LCD ribbon cable is at the top of the display. + Default is 320x240 "landscape" orientation. + CONFIG_LCD_BACKLIGHT - Define to support a backlight. + CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an + adjustable backlight will be provided using timer 1 to generate + various pulse widthes. The granularity of the settings is + determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or + CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight + is provided. + CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears + to be a shift in the returned data. This value fixes the offset. + Default 5. + + The LCD driver dynamically selects the LCD based on the reported LCD + ID value. However, code size can be reduced by suppressing support for + individual LCDs using: + + CONFIG_STM32_AM240320_DISABLE + CONFIG_STM32_SPFD5408B_DISABLE + CONFIG_STM32_R61580_DISABLE + +Configurations +============== + +Each STM3210E-EVAL configuration is maintained in a sudirectory and +can be selected as follow: + + cd tools + ./configure.sh stm3210e-eval/ + cd - + . ./setenv.sh + +Where is one of the following: + + buttons: + -------- + + Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and + button interrupts. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + composite + --------- + + This configuration exercises a composite USB interface consisting + of a CDC/ACM device and a USB mass storage device. This configuration + uses apps/examples/composite. + + nsh and nsh2: + ------------ + Configure the NuttShell (nsh) located at examples/nsh. + + Differences between the two NSH configurations: + + =========== ======================= ================================ + nsh nsh2 + =========== ======================= ================================ + Toolchain: NuttX buildroot for Codesourcery for Windows (1) + Linux or Cygwin (1,2) + ----------- ----------------------- -------------------------------- + Loader: DfuSe DfuSe + ----------- ----------------------- -------------------------------- + Serial Debug output: USART1 Debug output: USART1 + Console: NSH output: USART1 NSH output: USART1 (3) + ----------- ----------------------- -------------------------------- + microSD Yes Yes + Support + ----------- ----------------------- -------------------------------- + FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y + Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) + ----------- ----------------------- -------------------------------- + Support for No Yes + Built-in + Apps + ----------- ----------------------- -------------------------------- + Built-in None apps/examples/nx + Apps apps/examples/nxhello + apps/examples/usbstorage (5) + =========== ======================= ================================ + + (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh + to set up the correct PATH variable for whichever toolchain you + may use. + (2) Since DfuSe is assumed, this configuration may only work under + Cygwin without modification. + (3) When any other device other than /dev/console is used for a user + interface, (1) linefeeds (\n) will not be expanded to carriage return + / linefeeds \r\n). You will need to configure your terminal program + to account for this. And (2) input is not automatically echoed so + you will have to turn local echo on. + (4) Microsoft holds several patents related to the design of + long file names in the FAT file system. Please refer to the + details in the top-level COPYING file. Please do not use FAT + long file name unless you are familiar with these patent issues. + (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), + Caution should be used to assure that the SD drive is not in use when + the USB storage device is configured. Specifically, the SD driver + should be unmounted like: + + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH + ... + nsh> umount /mnd/sdcard # Unmount before connecting USB!!! + nsh> msconn # Connect the USB storage device + ... + nsh> msdis # Disconnect USB storate device + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount + + Failure to do this could result in corruption of the SD card format. + + nx: + --- + An example using the NuttX graphics system (NX). This example + focuses on general window controls, movement, mouse and keyboard + input. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxlines: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing lines on the background in various + orientations. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + nxtext: + ------ + Another example using the NuttX graphics system (NX). This + example focuses on placing text on the background while pop-up + windows occur. Text should continue to update normally with + or without the popup windows present. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait + + NOTE: When I tried building this example with the CodeSourcery + tools, I got a hardfault inside of its libgcc. I haven't + retested since then, but beware if you choose to change the + toolchain. + + ostest: + ------ + This configuration directory, performs a simple OS test using + examples/ostest. By default, this project assumes that you are + using the DFU bootloader. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + RIDE + ---- + This configuration builds a trivial bring-up binary. It is + useful only because it words with the RIDE7 IDE and R-Link debugger. + + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + + usbserial: + --------- + This configuration directory exercises the USB serial class + driver at examples/usbserial. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + + USB debug output can be enabled as by changing the following + settings in the configuration file: + + -CONFIG_DEBUG=n + -CONFIG_DEBUG_VERBOSE=n + -CONFIG_DEBUG_USB=n + +CONFIG_DEBUG=y + +CONFIG_DEBUG_VERBOSE=y + +CONFIG_DEBUG_USB=y + + -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n + -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n + -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n + +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y + +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y + +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y + + By default, the usbserial example uses the Prolific PL2303 + serial/USB converter emulation. The example can be modified + to use the CDC/ACM serial class by making the following changes + to the configuration file: + + -CONFIG_PL2303=y + +CONFIG_PL2303=n + + -CONFIG_CDCACM=n + +CONFIG_CDCACM=y + + The example can also be converted to use the alternative + USB serial example at apps/examples/usbterm by changing the + following: + + -CONFIGURED_APPS += examples/usbserial + +CONFIGURED_APPS += examples/usbterm + + In either the original appconfig file (before configuring) + or in the final apps/.config file (after configuring). + + usbstorage: + ---------- + This configuration directory exercises the USB mass storage + class driver at examples/usbstorage. See examples/README.txt for + more information. + + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin + diff --git a/nuttx-configs/px4io-v1/common/Make.defs b/nuttx-configs/px4io-v1/common/Make.defs new file mode 100644 index 000000000..74b183067 --- /dev/null +++ b/nuttx-configs/px4io-v1/common/Make.defs @@ -0,0 +1,171 @@ +############################################################################ +# configs/px4fmu/common/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# +# Generic Make.defs for the PX4FMU +# Do not specify/use this file directly - it is included by config-specific +# Make.defs in the per-config directories. +# + +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4io-v1/common/ld.script b/nuttx-configs/px4io-v1/common/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx-configs/px4io-v1/common/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v1/common/setenv.sh b/nuttx-configs/px4io-v1/common/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx-configs/px4io-v1/common/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/include/README.txt b/nuttx-configs/px4io-v1/include/README.txt new file mode 100755 index 000000000..2264a80aa --- /dev/null +++ b/nuttx-configs/px4io-v1/include/README.txt @@ -0,0 +1 @@ +This directory contains header files unique to the PX4IO board. diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h new file mode 100755 index 000000000..815079266 --- /dev/null +++ b/nuttx-configs/px4io-v1/include/board.h @@ -0,0 +1,152 @@ +/************************************************************************************ + * configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * 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 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +# include +#endif +#include +#include +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 24000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + +#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#undef GPIO_USART2_CTS +#define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS +#define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK +#define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_TX +#define GPIO_USART3_TX 0xffffffff +#undef GPIO_USART3_CK +#define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS +#define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS +#define GPIO_USART3_RTS 0xffffffff + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4io-v1/include/drv_i2c_device.h b/nuttx-configs/px4io-v1/include/drv_i2c_device.h new file mode 100644 index 000000000..02582bc09 --- /dev/null +++ b/nuttx-configs/px4io-v1/include/drv_i2c_device.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); +extern bool i2c_fsm(void); diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs new file mode 100644 index 000000000..c7f6effd9 --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4io-v1/common/Make.defs diff --git a/nuttx-configs/px4io-v1/nsh/appconfig b/nuttx-configs/px4io-v1/nsh/appconfig new file mode 100644 index 000000000..48a41bcdb --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/appconfig @@ -0,0 +1,32 @@ +############################################################################ +# +# 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. +# +############################################################################ diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig new file mode 100755 index 000000000..525672233 --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -0,0 +1,559 @@ +############################################################################ +# configs/px4io-v1/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F100C8=y +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4IO_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4io-v1" +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARMV7M_STACKCHECK is not set + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set +# CONFIG_NSH_BUILTIN_APPS is not set diff --git a/nuttx-configs/px4io-v1/nsh/setenv.sh b/nuttx-configs/px4io-v1/nsh/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/src/Makefile b/nuttx-configs/px4io-v1/src/Makefile new file mode 100644 index 000000000..bb9539d16 --- /dev/null +++ b/nuttx-configs/px4io-v1/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/stm3210e-eval/src/Makefile +# +# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx-configs/px4io-v1/src/README.txt b/nuttx-configs/px4io-v1/src/README.txt new file mode 100644 index 000000000..d4eda82fd --- /dev/null +++ b/nuttx-configs/px4io-v1/src/README.txt @@ -0,0 +1 @@ +This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v1/src/drv_i2c_device.c b/nuttx-configs/px4io-v1/src/drv_i2c_device.c new file mode 100644 index 000000000..1f5931ae5 --- /dev/null +++ b/nuttx-configs/px4io-v1/src/drv_i2c_device.c @@ -0,0 +1,618 @@ +/**************************************************************************** + * + * 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 A simple, polled I2C slave-mode driver. + * + * The master writes to and reads from a byte buffer, which the caller + * can update inbetween calls to the FSM. + */ + +#include + +#include "stm32_i2c.h" + +#include + +/* + * I2C register definitions. + */ +#define I2C_BASE STM32_I2C1_BASE + +#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) + +#define rCR1 REG(STM32_I2C_CR1_OFFSET) +#define rCR2 REG(STM32_I2C_CR2_OFFSET) +#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) +#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) +#define rDR REG(STM32_I2C_DR_OFFSET) +#define rSR1 REG(STM32_I2C_SR1_OFFSET) +#define rSR2 REG(STM32_I2C_SR2_OFFSET) +#define rCCR REG(STM32_I2C_CCR_OFFSET) +#define rTRISE REG(STM32_I2C_TRISE_OFFSET) + +/* + * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib + */ +#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ +#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ +#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ +#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ +#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ +#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ + +/** + * States implemented by the I2C FSM. + */ +enum fsm_state { + BAD_PHASE, // must be zero, default exit on a bad state transition + + WAIT_FOR_MASTER, + + /* write from master */ + WAIT_FOR_COMMAND, + RECEIVE_COMMAND, + RECEIVE_DATA, + HANDLE_COMMAND, + + /* read from master */ + WAIT_TO_SEND, + SEND_STATUS, + SEND_DATA, + + NUM_STATES +}; + +/** + * Events recognised by the I2C FSM. + */ +enum fsm_event { + /* automatic transition */ + AUTO, + + /* write from master */ + ADDRESSED_WRITE, + BYTE_RECEIVED, + STOP_RECEIVED, + + /* read from master */ + ADDRESSED_READ, + BYTE_SENDABLE, + ACK_FAILED, + + NUM_EVENTS +}; + +/** + * Context for the I2C FSM + */ +static struct fsm_context { + enum fsm_state state; + + /* XXX want to eliminate these */ + uint8_t command; + uint8_t status; + + uint8_t *data_ptr; + uint32_t data_count; + + size_t buffer_size; + uint8_t *buffer; +} context; + +/** + * Structure defining one FSM state and its outgoing transitions. + */ +struct fsm_transition { + void (*handler)(void); + enum fsm_state next_state[NUM_EVENTS]; +}; + +static bool i2c_command_received; + +static void fsm_event(enum fsm_event event); + +static void go_bad(void); +static void go_wait_master(void); + +static void go_wait_command(void); +static void go_receive_command(void); +static void go_receive_data(void); +static void go_handle_command(void); + +static void go_wait_send(void); +static void go_send_status(void); +static void go_send_buffer(void); + +/** + * The FSM state graph. + */ +static const struct fsm_transition fsm[NUM_STATES] = { + [BAD_PHASE] = { + .handler = go_bad, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + [WAIT_FOR_MASTER] = { + .handler = go_wait_master, + .next_state = { + [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, + [ADDRESSED_READ] = WAIT_TO_SEND, + }, + }, + + /* write from master*/ + [WAIT_FOR_COMMAND] = { + .handler = go_wait_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_COMMAND, + [STOP_RECEIVED] = WAIT_FOR_MASTER, + }, + }, + [RECEIVE_COMMAND] = { + .handler = go_receive_command, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [RECEIVE_DATA] = { + .handler = go_receive_data, + .next_state = { + [BYTE_RECEIVED] = RECEIVE_DATA, + [STOP_RECEIVED] = HANDLE_COMMAND, + }, + }, + [HANDLE_COMMAND] = { + .handler = go_handle_command, + .next_state = { + [AUTO] = WAIT_FOR_MASTER, + }, + }, + + /* buffer send */ + [WAIT_TO_SEND] = { + .handler = go_wait_send, + .next_state = { + [BYTE_SENDABLE] = SEND_STATUS, + }, + }, + [SEND_STATUS] = { + .handler = go_send_status, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, + [SEND_DATA] = { + .handler = go_send_buffer, + .next_state = { + [BYTE_SENDABLE] = SEND_DATA, + [ACK_FAILED] = WAIT_FOR_MASTER, + }, + }, +}; + + +/* debug support */ +#if 1 +struct fsm_logentry { + char kind; + uint32_t code; +}; + +#define LOG_ENTRIES 32 +static struct fsm_logentry fsm_log[LOG_ENTRIES]; +int fsm_logptr; +#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) +#define LOGx(_kind, _code) \ + do { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr = LOG_NEXT(fsm_logptr); \ + fsm_log[fsm_logptr].kind = 0; \ + } while(0) + +#define LOG(_kind, _code) \ + do {\ + if (fsm_logptr < LOG_ENTRIES) { \ + fsm_log[fsm_logptr].kind = _kind; \ + fsm_log[fsm_logptr].code = _code; \ + fsm_logptr++;\ + }\ + }while(0) + +#else +#define LOG(_kind, _code) +#endif + + +static void i2c_setclock(uint32_t frequency); + +/** + * Initialise I2C + * + */ +void +i2c_fsm_init(uint8_t *buffer, size_t buffer_size) +{ + /* save the buffer */ + context.buffer = buffer; + context.buffer_size = buffer_size; + + // initialise the FSM + context.status = 0; + context.command = 0; + context.state = BAD_PHASE; + fsm_event(AUTO); + +#if 0 + // enable the i2c block clock and reset it + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + + // configure the i2c GPIOs + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + + // set the peripheral clock to match the APB clock + rCR2 = STM32_PCLK1_FREQUENCY / 1000000; + + // configure for 100kHz operation + i2c_setclock(100000); + + // enable i2c + rCR1 = I2C_CR1_PE; +#endif +} + +/** + * Run the I2C FSM for some period. + * + * @return True if the buffer has been updated by a command. + */ +bool +i2c_fsm(void) +{ + uint32_t event; + int idle_iterations = 0; + + for (;;) { + // handle bus error states by discarding the current operation + if (rSR1 & I2C_SR1_BERR) { + context.state = WAIT_FOR_MASTER; + rSR1 = ~I2C_SR1_BERR; + } + + // we do not anticipate over/underrun errors as clock-stretching is enabled + + // fetch the most recent event + event = ((rSR2 << 16) | rSR1) & 0x00ffffff; + + // generate FSM events based on I2C events + switch (event) { + case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: + LOG('w', 0); + fsm_event(ADDRESSED_WRITE); + break; + + case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: + LOG('r', 0); + fsm_event(ADDRESSED_READ); + break; + + case I2C_EVENT_SLAVE_BYTE_RECEIVED: + LOG('R', 0); + fsm_event(BYTE_RECEIVED); + break; + + case I2C_EVENT_SLAVE_STOP_DETECTED: + LOG('s', 0); + fsm_event(STOP_RECEIVED); + break; + + case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: + //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: + LOG('T', 0); + fsm_event(BYTE_SENDABLE); + break; + + case I2C_EVENT_SLAVE_ACK_FAILURE: + LOG('a', 0); + fsm_event(ACK_FAILED); + break; + + default: + idle_iterations++; +// if ((event) && (event != 0x00020000)) +// LOG('e', event); + break; + } + + /* if we have just received something, drop out and let the caller handle it */ + if (i2c_command_received) { + i2c_command_received = false; + return true; + } + + /* if we have done nothing recently, drop out and let the caller have a slice */ + if (idle_iterations > 1000) + return false; + } +} + +/** + * Update the FSM with an event + * + * @param event New event. + */ +static void +fsm_event(enum fsm_event event) +{ + // move to the next state + context.state = fsm[context.state].next_state[event]; + + LOG('f', context.state); + + // call the state entry handler + if (fsm[context.state].handler) { + fsm[context.state].handler(); + } +} + +static void +go_bad() +{ + LOG('B', 0); + fsm_event(AUTO); +} + +/** + * Wait for the master to address us. + * + */ +static void +go_wait_master() +{ + // We currently don't have a command byte. + // + context.command = '\0'; + + // The data pointer starts pointing to the start of the data buffer. + // + context.data_ptr = context.buffer; + + // The data count is either: + // - the size of the data buffer + // - some value less than or equal the size of the data buffer during a write or a read + // + context.data_count = context.buffer_size; + + // (re)enable the peripheral, clear the stop event flag in + // case we just finished receiving data + rCR1 |= I2C_CR1_PE; + + // clear the ACK failed flag in case we just finished sending data + rSR1 = ~I2C_SR1_AF; +} + +/** + * Prepare to receive a command byte. + * + */ +static void +go_wait_command() +{ + // NOP +} + +/** + * Command byte has been received, save it and prepare to handle the data. + * + */ +static void +go_receive_command() +{ + + // fetch the command byte + context.command = (uint8_t)rDR; + LOG('c', context.command); + +} + +/** + * Receive a data byte. + * + */ +static void +go_receive_data() +{ + uint8_t d; + + // fetch the byte + d = (uint8_t)rDR; + LOG('d', d); + + // if we have somewhere to put it, do so + if (context.data_count) { + *context.data_ptr++ = d; + context.data_count--; + } +} + +/** + * Handle a command once the host is done sending it to us. + * + */ +static void +go_handle_command() +{ + // presume we are happy with the command + context.status = 0; + + // make a note that the buffer contains a fresh command + i2c_command_received = true; + + // kick along to the next state + fsm_event(AUTO); +} + +/** + * Wait to be able to send the status byte. + * + */ +static void +go_wait_send() +{ + // NOP +} + +/** + * Send the status byte. + * + */ +static void +go_send_status() +{ + rDR = context.status; + LOG('?', context.status); +} + +/** + * Send a data or pad byte. + * + */ +static void +go_send_buffer() +{ + if (context.data_count) { + LOG('D', *context.data_ptr); + rDR = *(context.data_ptr++); + context.data_count--; + } else { + LOG('-', 0); + rDR = 0xff; + } +} + +/* cribbed directly from the NuttX master driver */ +static void +i2c_setclock(uint32_t frequency) +{ + uint16_t cr1; + uint16_t ccr; + uint16_t trise; + uint16_t freqmhz; + uint16_t speed; + + /* Disable the selected I2C peripheral to configure TRISE */ + + cr1 = rCR1; + rCR1 &= ~I2C_CR1_PE; + + /* Update timing and control registers */ + + freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); + ccr = 0; + + /* Configure speed in standard mode */ + + if (frequency <= 100000) { + /* Standard mode speed calculation */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); + + /* The CCR fault must be >= 4 */ + + if (speed < 4) { + /* Set the minimum allowed value */ + + speed = 4; + } + ccr |= speed; + + /* Set Maximum Rise Time for standard mode */ + + trise = freqmhz + 1; + + /* Configure speed in fast mode */ + } else { /* (frequency <= 400000) */ + /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ + +#ifdef CONFIG_I2C_DUTY16_9 + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); + + /* Set DUTY and fast speed bits */ + + ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); +#else + /* Fast mode speed calculation with Tlow/Thigh = 2 */ + + speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); + + /* Set fast speed bit */ + + ccr |= I2C_CCR_FS; +#endif + + /* Verify that the CCR speed value is nonzero */ + + if (speed < 1) { + /* Set the minimum allowed value */ + + speed = 1; + } + ccr |= speed; + + /* Set Maximum Rise Time for fast mode */ + + trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); + } + + /* Write the new values of the CCR and TRISE registers */ + + rCCR = ccr; + rTRISE = trise; + + /* Bit 14 of OAR1 must be configured and kept at 1 */ + + rOAR1 = I2C_OAR1_ONE); + + /* Re-enable the peripheral (or not) */ + + rCR1 = cr1; +} diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index b93ad4220..4b9c90638 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -112,34 +112,6 @@ #undef GPIO_USART3_RTS #define GPIO_USART3_RTS 0xffffffff -/* - * High-resolution timer - */ -#define HRT_TIMER 1 /* use timer1 for the HRT */ -#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ - -/* - * PPM - * - * PPM input is handled by the HRT timer. - * - * Pin is PA8, timer 1, channel 1 - */ -#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN GPIO_TIM1_CH1IN - -/* LED definitions ******************************************************************/ -/* PX4 has two LEDs that we will encode as: */ - -#define LED_STARTED 0 /* LED? */ -#define LED_HEAPALLOCATE 1 /* LED? */ -#define LED_IRQSENABLED 2 /* LED? + LED? */ -#define LED_STACKCREATED 3 /* LED? */ -#define LED_INIRQ 4 /* LED? + LED? */ -#define LED_SIGNAL 5 /* LED? + LED? */ -#define LED_ASSERTION 6 /* LED? + LED? + LED? */ -#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ - /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 8d2eb056e..b4c5fa06e 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -92,12 +92,6 @@ #include -__BEGIN_DECLS -#include -__END_DECLS -#include -#include - #include #include #include @@ -107,15 +101,19 @@ __END_DECLS #include #include #include - -#include +#include #include #include #include +#include + +#include + +#include +#include -#include #include #include #include diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 4409a8a9c..cfb625670 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h new file mode 100644 index 000000000..9d7c81f85 --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -0,0 +1,209 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS2" + +//#ifdef CONFIG_STM32_SPI2 +//# error "SPI2 is not supported on this board" +//#endif + +#if defined(CONFIG_STM32_CAN1) +# warning "CAN1 is not supported on this board" +#endif + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + +/* External interrupts */ +#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL 2 +#define PX4_SPIDEV_MPU 3 + +/* + * Optional devices on IO's external port + */ +#define PX4_SPIDEV_ACCEL_MAG 2 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_MS5611 0x76 +#define PX4_I2C_OBDEV_EEPROM NOTDEFINED + +#define PX4_I2C_OBDEV_PX4IO_BL 0x18 +#define PX4_I2C_OBDEV_PX4IO 0x1a + +/* User GPIOs + * + * GPIO0-1 are the buffered high-power GPIOs. + * GPIO2-5 are the USART2 pins. + * GPIO6-7 are the CAN2 pins. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) +#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 3 /* timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) + +/* + * PWM + * + * Four PWM outputs can be configured on pins otherwise shared with + * USART2; two can take the flow control pins if they are not being used. + * + * Pins: + * + * CTS - PA0 - TIM2CH1 + * RTS - PA1 - TIM2CH2 + * TX - PA2 - TIM2CH3 + * RX - PA3 - TIM2CH4 + * + */ +#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1 +#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 +#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 +#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk new file mode 100644 index 000000000..66b776917 --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/module.mk @@ -0,0 +1,10 @@ +# +# Board-specific startup code for the PX4FMU +# + +SRCS = px4fmu_can.c \ + px4fmu_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c \ + px4fmu_led.c diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c new file mode 100644 index 000000000..1e1f10040 --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c new file mode 100644 index 000000000..964f5069c --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "stm32.h" +#include "board_config.h" +#include "stm32_uart.h" + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs (empty call to NuttX' ledinit) */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct spi_dev_s *spi3; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + int result; + + /* configure always-on ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + /* IN12 and IN13 further below */ + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + led_off(LED_BLUE); + + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\r\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\r\n"); + + /* + * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. + * Keep the SPI2 init optional and conditionally initialize the ADC pins + */ + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + } else { + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); + + message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); + } + + /* Get the SPI port for the microSD slot */ + + message("[boot] Initializing SPI port 3\n"); + spi3 = up_spiinitialize(3); + + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully initialized SPI port 3\n"); + + /* Now bind the SPI interface to the MMCSD driver */ + result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c new file mode 100644 index 000000000..aa83ec130 --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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 px4fmu_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1-2 GPIOs for output */ + + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); +} + +__EXPORT void led_on(int led) +{ + if (led == 0) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED2, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 0) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED2, true); + } +} diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c new file mode 100644 index 000000000..848e21d79 --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c new file mode 100644 index 000000000..17e6862f7 --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32.h" +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL); + stm32_configgpio(GPIO_SPI_CS_MPU); + stm32_configgpio(GPIO_SPI_CS_SDCARD); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + break; + + case PX4_SPIDEV_ACCEL: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* this is actually bogus, but PX4 has no way to sense the presence of an SD card */ + return SPI_STATUS_PRESENT; +} + diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c new file mode 100644 index 000000000..0fc8569aa --- /dev/null +++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "stm32.h" +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h new file mode 100644 index 000000000..ec8dde499 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) + +/* External interrupts */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk new file mode 100644 index 000000000..99d37eeca --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -0,0 +1,10 @@ +# +# Board-specific startup code for the PX4FMUv2 +# + +SRCS = px4fmu_can.c \ + px4fmu2_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c \ + px4fmu2_led.c diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c new file mode 100644 index 000000000..135767b26 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -0,0 +1,262 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_SERVO_VALID); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\n"); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 375000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + + message("[boot] Successfully initialized SPI port 2\n"); + + #ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { + message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { + message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + + message("[boot] Initialized SDIO\n"); + #endif + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c new file mode 100644 index 000000000..5ded4294e --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c new file mode 100644 index 000000000..f66c7cd79 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c new file mode 100644 index 000000000..600dfef41 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c new file mode 100644 index 000000000..09838d02f --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c new file mode 100644 index 000000000..f329e06ff --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk deleted file mode 100644 index 66b776917..000000000 --- a/src/drivers/boards/px4fmu/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Board-specific startup code for the PX4FMU -# - -SRCS = px4fmu_can.c \ - px4fmu_init.c \ - px4fmu_pwm_servo.c \ - px4fmu_spi.c \ - px4fmu_usb.c \ - px4fmu_led.c diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c deleted file mode 100644 index 187689ff9..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_can.c +++ /dev/null @@ -1,144 +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 px4fmu_can.c - * - * Board-specific CAN functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include - -#include -#include - -#include "chip.h" -#include "up_arch.h" - -#include "stm32.h" -#include "stm32_can.h" -#include "px4fmu_internal.h" - -#ifdef CONFIG_CAN - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ -/* Configuration ********************************************************************/ - -#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) -# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." -# undef CONFIG_STM32_CAN2 -#endif - -#ifdef CONFIG_STM32_CAN1 -# define CAN_PORT 1 -#else -# define CAN_PORT 2 -#endif - -/* Debug ***************************************************************************/ -/* Non-standard debug that may be enabled just for testing CAN */ - -#ifdef CONFIG_DEBUG_CAN -# define candbg dbg -# define canvdbg vdbg -# define canlldbg lldbg -# define canllvdbg llvdbg -#else -# define candbg(x...) -# define canvdbg(x...) -# define canlldbg(x...) -# define canllvdbg(x...) -#endif - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: can_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/can. - * - ************************************************************************************/ - -int can_devinit(void) -{ - static bool initialized = false; - struct can_dev_s *can; - int ret; - - /* Check if we have already initialized */ - - if (!initialized) { - /* Call stm32_caninitialize() to get an instance of the CAN interface */ - - can = stm32_caninitialize(CAN_PORT); - - if (can == NULL) { - candbg("ERROR: Failed to get CAN interface\n"); - return -ENODEV; - } - - /* Register the CAN driver at "/dev/can0" */ - - ret = can_register("/dev/can0", can); - - if (ret < 0) { - candbg("ERROR: can_register failed: %d\n", ret); - return ret; - } - - /* Now we are initialized */ - - initialized = true; - } - - return OK; -} - -#endif diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c deleted file mode 100644 index 36af2298c..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ /dev/null @@ -1,268 +0,0 @@ -/**************************************************************************** - * - * 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 px4fmu_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "stm32.h" -#include "px4fmu_internal.h" -#include "stm32_uart.h" - -#include - -#include -#include - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/* - * Ideally we'd be able to get these from up_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(); -extern void led_on(int led); -extern void led_off(int led); -__END_DECLS - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - /* configure SPI interfaces */ - stm32_spiinitialize(); - - /* configure LEDs (empty call to NuttX' ledinit) */ - up_ledinit(); -} - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -static struct spi_dev_s *spi1; -static struct spi_dev_s *spi2; -static struct spi_dev_s *spi3; - -#include - -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif - -__EXPORT int nsh_archinitialize(void) -{ - int result; - - /* configure always-on ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - /* IN12 and IN13 further below */ - - /* configure the high-resolution time/callout interface */ - hrt_init(); - - /* configure CPU load estimation */ -#ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); -#endif - - /* set up the serial DMA polling */ - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - - /* initial LED state */ - drv_led_start(); - led_off(LED_AMBER); - led_off(LED_BLUE); - - - /* Configure SPI-based devices */ - - spi1 = up_spiinitialize(1); - - if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\r\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* Default SPI1 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false); - SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port 1\r\n"); - - /* - * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. - * Keep the SPI2 init optional and conditionally initialize the ADC pins - */ - spi2 = up_spiinitialize(2); - - if (!spi2) { - message("[boot] Enabling IN12/13 instead of SPI2\n"); - /* no SPI2, use pins for ADC */ - stm32_configgpio(GPIO_ADC1_IN12); - stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - } else { - /* Default SPI2 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 10000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); - - message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - } - - /* Get the SPI port for the microSD slot */ - - message("[boot] Initializing SPI port 3\n"); - spi3 = up_spiinitialize(3); - - if (!spi3) { - message("[boot] FAILED to initialize SPI port 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - message("[boot] Successfully initialized SPI port 3\n"); - - /* Now bind the SPI interface to the MMCSD driver */ - result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); - - if (result != OK) { - message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); - - return OK; -} diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h deleted file mode 100644 index 56173abf6..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ /dev/null @@ -1,165 +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 px4fmu_internal.h - * - * PX4FMU internal definitions - */ - -#pragma once - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -__BEGIN_DECLS - -/* these headers are not C++ safe */ -#include - - -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/* PX4IO connection configuration */ -#define PX4IO_SERIAL_DEVICE "/dev/ttyS2" - -//#ifdef CONFIG_STM32_SPI2 -//# error "SPI2 is not supported on this board" -//#endif - -#if defined(CONFIG_STM32_CAN1) -# warning "CAN1 is not supported on this board" -#endif - -/* PX4FMU GPIOs ***********************************************************************************/ -/* LEDs */ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) - -/* External interrupts */ -#define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) -// XXX MPU6000 DRDY? - -/* SPI chip selects */ - -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) -#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) -#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) - -/* User GPIOs - * - * GPIO0-1 are the buffered high-power GPIOs. - * GPIO2-5 are the USART2 pins. - * GPIO6-7 are the CAN2 pins. - */ -#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) -#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) -#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) -#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) -#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) -#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN13) -#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2) -#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) -#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) -#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) -#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) -#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) -#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) -#define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) - -/* USB OTG FS - * - * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) - */ -#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) - -/* PWM - * - * The PX4FMU has five PWM outputs, of which only the output on - * pin PC8 is fixed assigned to this function. The other four possible - * pwm sources are the TX, RX, RTS and CTS pins of USART2 - * - * Alternate function mapping: - * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 - */ - -#ifdef CONFIG_PWM -# if defined(CONFIG_STM32_TIM3_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 3 -# elif defined(CONFIG_STM32_TIM8_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 8 -# endif -#endif - -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - -#ifndef __ASSEMBLY__ - -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - -/**************************************************************************************************** - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ****************************************************************************************************/ - -extern void stm32_spiinitialize(void); - -#endif /* __ASSEMBLY__ */ - -__END_DECLS diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c deleted file mode 100644 index 31b25984e..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ /dev/null @@ -1,96 +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 px4fmu_led.c - * - * PX4FMU LED backend. - */ - -#include - -#include - -#include "stm32.h" -#include "px4fmu_internal.h" - -#include - -/* - * Ideally we'd be able to get these from up_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(); -extern void led_on(int led); -extern void led_off(int led); -__END_DECLS - -__EXPORT void led_init() -{ - /* Configure LED1-2 GPIOs for output */ - - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); -} - -__EXPORT void led_on(int led) -{ - if (led == 0) - { - /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); - } - if (led == 1) - { - /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED2, false); - } -} - -__EXPORT void led_off(int led) -{ - if (led == 0) - { - /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); - } - if (led == 1) - { - /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED2, true); - } -} diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c deleted file mode 100644 index d85131dd8..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ /dev/null @@ -1,87 +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 px4fmu_pwm_servo.c - * - * Configuration data for the stm32 pwm_servo driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include - -#include - -#include -#include - -#include -#include -#include - -__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { - { - .base = STM32_TIM2_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM2EN, - .clock_freq = STM32_APB1_TIM2_CLKIN - } -}; - -__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { - { - .gpio = GPIO_TIM2_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH3OUT, - .timer_index = 0, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH4OUT, - .timer_index = 0, - .timer_channel = 4, - .default_value = 1000, - } -}; diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c deleted file mode 100644 index e05ddecf3..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_spi.c +++ /dev/null @@ -1,154 +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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "chip.h" -#include "stm32.h" -#include "px4fmu_internal.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void weak_function stm32_spiinitialize(void) -{ - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL); - stm32_configgpio(GPIO_SPI_CS_MPU); - stm32_configgpio(GPIO_SPI_CS_SDCARD); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 1); -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); - break; - - case PX4_SPIDEV_ACCEL: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected); - stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - break; - - case PX4_SPIDEV_MPU: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); - break; - - default: - break; - - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - return SPI_STATUS_PRESENT; -} - -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - break; - - default: - break; - - } -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - return SPI_STATUS_PRESENT; -} - - -__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected); -} - -__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - /* this is actually bogus, but PX4 has no way to sense the presence of an SD card */ - return SPI_STATUS_PRESENT; -} - diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c deleted file mode 100644 index 0be981c1e..000000000 --- a/src/drivers/boards/px4fmu/px4fmu_usb.c +++ /dev/null @@ -1,108 +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 px4fmu_usb.c - * - * Board-specific USB functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include "up_arch.h" -#include "stm32.h" -#include "px4fmu_internal.h" - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_usbinitialize - * - * Description: - * Called to setup USB-related GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_usbinitialize(void) -{ - /* The OTG FS has an internal soft pull-up */ - - /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ - -#ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); - /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); - */ -#endif -} - -/************************************************************************************ - * Name: stm32_usbsuspend - * - * Description: - * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is - * used. This function is called whenever the USB enters or leaves suspend mode. - * This is an opportunity for the board logic to shutdown clocks, power, etc. - * while the USB is suspended. - * - ************************************************************************************/ - -__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) -{ - ulldbg("resume: %d\n", resume); -} - diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk deleted file mode 100644 index 99d37eeca..000000000 --- a/src/drivers/boards/px4fmuv2/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Board-specific startup code for the PX4FMUv2 -# - -SRCS = px4fmu_can.c \ - px4fmu2_init.c \ - px4fmu_pwm_servo.c \ - px4fmu_spi.c \ - px4fmu_usb.c \ - px4fmu2_led.c diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c deleted file mode 100644 index 13829d5c4..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu2_init.c +++ /dev/null @@ -1,262 +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 px4fmu_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include "px4fmu_internal.h" -#include - -#include - -#include -#include - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - /* configure SPI interfaces */ - stm32_spiinitialize(); - - /* configure LEDs */ - up_ledinit(); -} - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -static struct spi_dev_s *spi1; -static struct spi_dev_s *spi2; -static struct sdio_dev_s *sdio; - -#include - -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif - -__EXPORT int nsh_archinitialize(void) -{ - - /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ - stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ - stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ - stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ - stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ - - /* configure power supply control/sense pins */ - stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); - stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); - stm32_configgpio(GPIO_VDD_BRICK_VALID); - stm32_configgpio(GPIO_VDD_SERVO_VALID); - stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); - stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); - - /* configure the high-resolution time/callout interface */ - hrt_init(); - - /* configure CPU load estimation */ -#ifdef CONFIG_SCHED_INSTRUMENTATION - cpuload_initialize_once(); -#endif - - /* set up the serial DMA polling */ - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - - /* initial LED state */ - drv_led_start(); - led_off(LED_AMBER); - - /* Configure SPI-based devices */ - - spi1 = up_spiinitialize(1); - - if (!spi1) { - message("[boot] FAILED to initialize SPI port 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* Default SPI1 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi1, 10000000); - SPI_SETBITS(spi1, 8); - SPI_SETMODE(spi1, SPIDEV_MODE3); - SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); - SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); - up_udelay(20); - - message("[boot] Successfully initialized SPI port 1\n"); - - /* Get the SPI port for the FRAM */ - - spi2 = up_spiinitialize(2); - - if (!spi2) { - message("[boot] FAILED to initialize SPI port 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 375000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, SPIDEV_FLASH, false); - - message("[boot] Successfully initialized SPI port 2\n"); - - #ifdef CONFIG_MMCSD - /* First, get an instance of the SDIO interface */ - - sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); - if (!sdio) { - message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; - } - - /* Now bind the SDIO interface to the MMC/SD driver */ - int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); - if (ret != OK) { - message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; - } - - /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ - sdio_mediachange(sdio, true); - - message("[boot] Initialized SDIO\n"); - #endif - - return OK; -} diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c deleted file mode 100644 index 11a5c7211..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c +++ /dev/null @@ -1,85 +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 px4fmu2_led.c - * - * PX4FMU LED backend. - */ - -#include - -#include - -#include "stm32.h" -#include "px4fmu_internal.h" - -#include - -/* - * Ideally we'd be able to get these from up_internal.h, - * but since we want to be able to disable the NuttX use - * of leds for system indication at will and there is no - * separate switch, we need to build independent of the - * CONFIG_ARCH_LEDS configuration switch. - */ -__BEGIN_DECLS -extern void led_init(); -extern void led_on(int led); -extern void led_off(int led); -__END_DECLS - -__EXPORT void led_init() -{ - /* Configure LED1 GPIO for output */ - - stm32_configgpio(GPIO_LED1); -} - -__EXPORT void led_on(int led) -{ - if (led == 1) - { - /* Pull down to switch on */ - stm32_gpiowrite(GPIO_LED1, false); - } -} - -__EXPORT void led_off(int led) -{ - if (led == 1) - { - /* Pull up to switch off */ - stm32_gpiowrite(GPIO_LED1, true); - } -} diff --git a/src/drivers/boards/px4fmuv2/px4fmu_can.c b/src/drivers/boards/px4fmuv2/px4fmu_can.c deleted file mode 100644 index 86ba183b8..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu_can.c +++ /dev/null @@ -1,144 +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 px4fmu_can.c - * - * Board-specific CAN functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include - -#include -#include - -#include "chip.h" -#include "up_arch.h" - -#include "stm32.h" -#include "stm32_can.h" -#include "px4fmu_internal.h" - -#ifdef CONFIG_CAN - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ -/* Configuration ********************************************************************/ - -#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) -# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." -# undef CONFIG_STM32_CAN2 -#endif - -#ifdef CONFIG_STM32_CAN1 -# define CAN_PORT 1 -#else -# define CAN_PORT 2 -#endif - -/* Debug ***************************************************************************/ -/* Non-standard debug that may be enabled just for testing CAN */ - -#ifdef CONFIG_DEBUG_CAN -# define candbg dbg -# define canvdbg vdbg -# define canlldbg lldbg -# define canllvdbg llvdbg -#else -# define candbg(x...) -# define canvdbg(x...) -# define canlldbg(x...) -# define canllvdbg(x...) -#endif - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: can_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/can. - * - ************************************************************************************/ - -int can_devinit(void) -{ - static bool initialized = false; - struct can_dev_s *can; - int ret; - - /* Check if we have already initialized */ - - if (!initialized) { - /* Call stm32_caninitialize() to get an instance of the CAN interface */ - - can = stm32_caninitialize(CAN_PORT); - - if (can == NULL) { - candbg("ERROR: Failed to get CAN interface\n"); - return -ENODEV; - } - - /* Register the CAN driver at "/dev/can0" */ - - ret = can_register("/dev/can0", can); - - if (ret < 0) { - candbg("ERROR: can_register failed: %d\n", ret); - return ret; - } - - /* Now we are initialized */ - - initialized = true; - } - - return OK; -} - -#endif \ No newline at end of file diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h deleted file mode 100644 index ad66ce563..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h +++ /dev/null @@ -1,143 +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 px4fmu_internal.h - * - * PX4FMU internal definitions - */ - -#pragma once - -/**************************************************************************************************** - * Included Files - ****************************************************************************************************/ - -#include -#include -#include - -__BEGIN_DECLS - -/* these headers are not C++ safe */ -#include - - -/**************************************************************************************************** - * Definitions - ****************************************************************************************************/ -/* Configuration ************************************************************************************/ - -/* PX4IO connection configuration */ -#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" -#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX -#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX -#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ -#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 -#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 -#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 -#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY -#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ - - -/* PX4FMU GPIOs ***********************************************************************************/ -/* LEDs */ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) - -/* External interrupts */ - -/* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) - -/* User GPIOs - * - * GPIO0-5 are the PWM servo outputs. - */ -#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) -#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) -#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) -#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) -#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) -#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) -#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) -#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) -#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) -#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) - -/* Power supply control and monitoring GPIOs */ -#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) -#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) -#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) -#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) -#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) -#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) - -/* USB OTG FS - * - * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) - */ -#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) - -/**************************************************************************************************** - * Public Types - ****************************************************************************************************/ - -/**************************************************************************************************** - * Public data - ****************************************************************************************************/ - -#ifndef __ASSEMBLY__ - -/**************************************************************************************************** - * Public Functions - ****************************************************************************************************/ - -/**************************************************************************************************** - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ****************************************************************************************************/ - -extern void stm32_spiinitialize(void); - -#endif /* __ASSEMBLY__ */ - -__END_DECLS diff --git a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c deleted file mode 100644 index bcbc0010c..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu_pwm_servo.c +++ /dev/null @@ -1,105 +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 px4fmu_pwm_servo.c - * - * Configuration data for the stm32 pwm_servo driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include - -#include - -#include -#include - -#include -#include -#include - -__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { - { - .base = STM32_TIM1_BASE, - .clock_register = STM32_RCC_APB2ENR, - .clock_bit = RCC_APB2ENR_TIM1EN, - .clock_freq = STM32_APB2_TIM1_CLKIN - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN - } -}; - -__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { - { - .gpio = GPIO_TIM1_CH4OUT, - .timer_index = 0, - .timer_channel = 4, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM1_CH3OUT, - .timer_index = 0, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM1_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM1_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH2OUT, - .timer_index = 1, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH3OUT, - .timer_index = 1, - .timer_channel = 3, - .default_value = 1000, - } -}; diff --git a/src/drivers/boards/px4fmuv2/px4fmu_spi.c b/src/drivers/boards/px4fmuv2/px4fmu_spi.c deleted file mode 100644 index 5e90c227d..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu_spi.c +++ /dev/null @@ -1,141 +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 px4fmu_spi.c - * - * Board-specific SPI functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include "px4fmu_internal.h" - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_spiinitialize - * - * Description: - * Called to configure SPI chip select GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void weak_function stm32_spiinitialize(void) -{ -#ifdef CONFIG_STM32_SPI1 - stm32_configgpio(GPIO_SPI_CS_GYRO); - stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); - stm32_configgpio(GPIO_SPI_CS_BARO); - - /* De-activate all peripherals, - * required for some peripheral - * state machines - */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); -#endif - -#ifdef CONFIG_STM32_SPI2 - stm32_configgpio(GPIO_SPI_CS_FRAM); - stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); -#endif -} - -__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* SPI select is active low, so write !selected to select the device */ - - switch (devid) { - case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - break; - - case PX4_SPIDEV_ACCEL_MAG: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); - stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); - break; - - case PX4_SPIDEV_BARO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); - stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); - stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); - break; - - default: - break; - } -} - -__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - return SPI_STATUS_PRESENT; -} - - -#ifdef CONFIG_STM32_SPI2 -__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) -{ - /* there can only be one device on this bus, so always select it */ - stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); -} - -__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) -{ - /* FRAM is always present */ - return SPI_STATUS_PRESENT; -} -#endif diff --git a/src/drivers/boards/px4fmuv2/px4fmu_usb.c b/src/drivers/boards/px4fmuv2/px4fmu_usb.c deleted file mode 100644 index 3492e2c68..000000000 --- a/src/drivers/boards/px4fmuv2/px4fmu_usb.c +++ /dev/null @@ -1,108 +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 px4fmu_usb.c - * - * Board-specific USB functions. - */ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include "px4fmu_internal.h" - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: stm32_usbinitialize - * - * Description: - * Called to setup USB-related GPIO pins for the PX4FMU board. - * - ************************************************************************************/ - -__EXPORT void stm32_usbinitialize(void) -{ - /* The OTG FS has an internal soft pull-up */ - - /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ - -#ifdef CONFIG_STM32_OTGFS - stm32_configgpio(GPIO_OTGFS_VBUS); - /* XXX We only support device mode - stm32_configgpio(GPIO_OTGFS_PWRON); - stm32_configgpio(GPIO_OTGFS_OVER); - */ -#endif -} - -/************************************************************************************ - * Name: stm32_usbsuspend - * - * Description: - * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is - * used. This function is called whenever the USB enters or leaves suspend mode. - * This is an opportunity for the board logic to shutdown clocks, power, etc. - * while the USB is suspended. - * - ************************************************************************************/ - -__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) -{ - ulldbg("resume: %d\n", resume); -} - diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h new file mode 100644 index 000000000..48aadbd76 --- /dev/null +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4IO hardware definitions. + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include +#include + +/* these headers are not C++ safe */ +#include +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* PX4IO GPIOs **********************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) + +/* Safety switch button *************************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ************************************************************/ + +#define GPIO_ACC1_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_ACC2_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12) +#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) + +#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) +#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) + +/* Analog inputs ********************************************************************/ + +#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) +#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk new file mode 100644 index 000000000..2601a1c15 --- /dev/null +++ b/src/drivers/boards/px4io-v1/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IO +# + +SRCS = px4io_init.c \ + px4io_pwm_servo.c diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c new file mode 100644 index 000000000..8292da9e1 --- /dev/null +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 px4io_init.c + * + * PX4IO-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "stm32.h" +#include "board_config.h" +#include "stm32_uart.h" + +#include + +#include +#include +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure GPIOs */ + stm32_configgpio(GPIO_ACC1_PWR_EN); + stm32_configgpio(GPIO_ACC2_PWR_EN); + stm32_configgpio(GPIO_SERVO_PWR_EN); + stm32_configgpio(GPIO_RELAY1_EN); + stm32_configgpio(GPIO_RELAY2_EN); + + /* turn off - all leds are active low */ + stm32_gpiowrite(GPIO_LED1, true); + stm32_gpiowrite(GPIO_LED2, true); + stm32_gpiowrite(GPIO_LED3, true); + + /* LED config */ + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + + stm32_configgpio(GPIO_ACC_OC_DETECT); + stm32_configgpio(GPIO_SERVO_OC_DETECT); + stm32_configgpio(GPIO_BTN_SAFETY); + + stm32_configgpio(GPIO_ADC_VBATT); + stm32_configgpio(GPIO_ADC_IN5); +} diff --git a/src/drivers/boards/px4io-v1/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c new file mode 100644 index 000000000..6df470da6 --- /dev/null +++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h new file mode 100644 index 000000000..818b64436 --- /dev/null +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include +#include +#include + +/* these headers are not C++ safe */ +#include +#include + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * Serial + ******************************************************************************/ +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 1500000 + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) + +/* Safety switch button *******************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) + +/* Analog inputs **************************************************************/ + +#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) + +/* the same rssi signal goes to both an adc and a timer input */ +#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) +#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8) + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +/* XXX these should be UART pins */ +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk new file mode 100644 index 000000000..85f94e8be --- /dev/null +++ b/src/drivers/boards/px4io-v2/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c \ + px4iov2_pwm_servo.c diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c new file mode 100644 index 000000000..0ea95bded --- /dev/null +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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 px4iov2_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" + +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + + /* configure GPIOs */ + + /* LEDS - default to off */ + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - disable it by default */ + /* XXX might not want to do this on warm restart? */ + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + + /* RSSI inputs */ + stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ + stm32_configgpio(GPIO_ADC_RSSI); + + /* servo rail voltage */ + stm32_configgpio(GPIO_ADC_VSERVO); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); + stm32_configgpio(GPIO_SBUS_OUTPUT); + + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(GPIO_SBUS_OENABLE, true); + stm32_configgpio(GPIO_SBUS_OENABLE); + + stm32_configgpio(GPIO_PPM); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_PWM1, false); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, false); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, false); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, false); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, false); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, false); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, false); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, false); + stm32_configgpio(GPIO_PWM8); +} diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c new file mode 100644 index 000000000..4f1b9487c --- /dev/null +++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 px4iov2_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io/module.mk deleted file mode 100644 index 2601a1c15..000000000 --- a/src/drivers/boards/px4io/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# Board-specific startup code for the PX4IO -# - -SRCS = px4io_init.c \ - px4io_pwm_servo.c diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c deleted file mode 100644 index 15c59e423..000000000 --- a/src/drivers/boards/px4io/px4io_init.c +++ /dev/null @@ -1,106 +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 px4io_init.c - * - * PX4IO-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include - -#include "stm32.h" -#include "px4io_internal.h" -#include "stm32_uart.h" - -#include - -#include -#include -#include - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - /* configure GPIOs */ - stm32_configgpio(GPIO_ACC1_PWR_EN); - stm32_configgpio(GPIO_ACC2_PWR_EN); - stm32_configgpio(GPIO_SERVO_PWR_EN); - stm32_configgpio(GPIO_RELAY1_EN); - stm32_configgpio(GPIO_RELAY2_EN); - - /* turn off - all leds are active low */ - stm32_gpiowrite(GPIO_LED1, true); - stm32_gpiowrite(GPIO_LED2, true); - stm32_gpiowrite(GPIO_LED3, true); - - /* LED config */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); - - stm32_configgpio(GPIO_ACC_OC_DETECT); - stm32_configgpio(GPIO_SERVO_OC_DETECT); - stm32_configgpio(GPIO_BTN_SAFETY); - - stm32_configgpio(GPIO_ADC_VBATT); - stm32_configgpio(GPIO_ADC_IN5); -} diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h deleted file mode 100644 index 6638e715e..000000000 --- a/src/drivers/boards/px4io/px4io_internal.h +++ /dev/null @@ -1,85 +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 px4io_internal.h - * - * PX4IO hardware definitions. - */ - -#pragma once - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include -#include -#include - -#include - -/************************************************************************************ - * Definitions - ************************************************************************************/ - -/* PX4IO GPIOs **********************************************************************/ -/* LEDs */ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) - -/* Safety switch button *************************************************************/ - -#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) - -/* Power switch controls ************************************************************/ - -#define GPIO_ACC1_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) -#define GPIO_ACC2_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) -#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) - -#define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12) -#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) - -#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) -#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) - -/* Analog inputs ********************************************************************/ - -#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) -#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c deleted file mode 100644 index 6df470da6..000000000 --- a/src/drivers/boards/px4io/px4io_pwm_servo.c +++ /dev/null @@ -1,123 +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 px4fmu_pwm_servo.c - * - * Configuration data for the stm32 pwm_servo driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include - -#include - -#include -#include - -#include -#include -#include - -__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { - { - .base = STM32_TIM2_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM2EN, - .clock_freq = STM32_APB1_TIM2_CLKIN - }, - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN - } -}; - -__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { - { - .gpio = GPIO_TIM2_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH3OUT, - .timer_index = 2, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH4OUT, - .timer_index = 2, - .timer_channel = 4, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH1OUT, - .timer_index = 1, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH2OUT, - .timer_index = 1, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH3OUT, - .timer_index = 1, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH4OUT, - .timer_index = 1, - .timer_channel = 4, - .default_value = 1000, - } -}; diff --git a/src/drivers/boards/px4iov2/module.mk b/src/drivers/boards/px4iov2/module.mk deleted file mode 100644 index 85f94e8be..000000000 --- a/src/drivers/boards/px4iov2/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# Board-specific startup code for the PX4IOv2 -# - -SRCS = px4iov2_init.c \ - px4iov2_pwm_servo.c diff --git a/src/drivers/boards/px4iov2/px4iov2_init.c b/src/drivers/boards/px4iov2/px4iov2_init.c deleted file mode 100644 index e95298bf5..000000000 --- a/src/drivers/boards/px4iov2/px4iov2_init.c +++ /dev/null @@ -1,162 +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 px4iov2_init.c - * - * PX4FMU-specific early startup code. This file implements the - * nsh_archinitialize() function that is called early by nsh during startup. - * - * Code here is run before the rcS script is invoked; it should start required - * subsystems and perform board-specific initialisation. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include - -#include -#include "px4iov2_internal.h" - -#include - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Debug ********************************************************************/ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# define message(...) lowsyslog(__VA_ARGS__) -# else -# define message(...) printf(__VA_ARGS__) -# endif -#else -# ifdef CONFIG_DEBUG -# define message lowsyslog -# else -# define message printf -# endif -#endif - -/**************************************************************************** - * Protected Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: stm32_boardinitialize - * - * Description: - * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured - * and mapped but before any devices have been initialized. - * - ************************************************************************************/ - -__EXPORT void stm32_boardinitialize(void) -{ - - /* configure GPIOs */ - - /* LEDS - default to off */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); - - stm32_configgpio(GPIO_BTN_SAFETY); - - /* spektrum power enable is active high - disable it by default */ - /* XXX might not want to do this on warm restart? */ - stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); - stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); - - stm32_configgpio(GPIO_SERVO_FAULT_DETECT); - - /* RSSI inputs */ - stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ - stm32_configgpio(GPIO_ADC_RSSI); - - /* servo rail voltage */ - stm32_configgpio(GPIO_ADC_VSERVO); - - stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); - stm32_configgpio(GPIO_SBUS_OUTPUT); - - /* sbus output enable is active low - disable it by default */ - stm32_gpiowrite(GPIO_SBUS_OENABLE, true); - stm32_configgpio(GPIO_SBUS_OENABLE); - - stm32_configgpio(GPIO_PPM); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_PWM1, false); - stm32_configgpio(GPIO_PWM1); - - stm32_gpiowrite(GPIO_PWM2, false); - stm32_configgpio(GPIO_PWM2); - - stm32_gpiowrite(GPIO_PWM3, false); - stm32_configgpio(GPIO_PWM3); - - stm32_gpiowrite(GPIO_PWM4, false); - stm32_configgpio(GPIO_PWM4); - - stm32_gpiowrite(GPIO_PWM5, false); - stm32_configgpio(GPIO_PWM5); - - stm32_gpiowrite(GPIO_PWM6, false); - stm32_configgpio(GPIO_PWM6); - - stm32_gpiowrite(GPIO_PWM7, false); - stm32_configgpio(GPIO_PWM7); - - stm32_gpiowrite(GPIO_PWM8, false); - stm32_configgpio(GPIO_PWM8); -} diff --git a/src/drivers/boards/px4iov2/px4iov2_internal.h b/src/drivers/boards/px4iov2/px4iov2_internal.h deleted file mode 100755 index 2bf65e047..000000000 --- a/src/drivers/boards/px4iov2/px4iov2_internal.h +++ /dev/null @@ -1,118 +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 px4iov2_internal.h - * - * PX4IOV2 internal definitions - */ - -#pragma once - -/****************************************************************************** - * Included Files - ******************************************************************************/ - -#include -#include -#include - -/* these headers are not C++ safe */ -#include - - -/****************************************************************************** - * Definitions - ******************************************************************************/ -/* Configuration **************************************************************/ - -/****************************************************************************** - * Serial - ******************************************************************************/ -#define PX4FMU_SERIAL_BASE STM32_USART2_BASE -#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 -#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX -#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX -#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX -#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX -#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY -#define PX4FMU_SERIAL_BITRATE 1500000 - -/****************************************************************************** - * GPIOS - ******************************************************************************/ - -/* LEDS **********************************************************************/ - -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) - -/* Safety switch button *******************************************************/ - -#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) - -/* Power switch controls ******************************************************/ - -#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) - -#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) - -/* Analog inputs **************************************************************/ - -#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) - -/* the same rssi signal goes to both an adc and a timer input */ -#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) -#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) - -/* PWM pins **************************************************************/ - -#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8) - -#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) -#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) -#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) -#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) -#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) -#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) -#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) -#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) - -/* SBUS pins *************************************************************/ - -/* XXX these should be UART pins */ -#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) -#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) - diff --git a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c b/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c deleted file mode 100644 index 4f1b9487c..000000000 --- a/src/drivers/boards/px4iov2/px4iov2_pwm_servo.c +++ /dev/null @@ -1,123 +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 px4iov2_pwm_servo.c - * - * Configuration data for the stm32 pwm_servo driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include - -#include - -#include -#include - -#include -#include -#include - -__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { - { - .base = STM32_TIM2_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM2EN, - .clock_freq = STM32_APB1_TIM2_CLKIN - }, - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN - } -}; - -__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { - { - .gpio = GPIO_TIM2_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH3OUT, - .timer_index = 2, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH4OUT, - .timer_index = 2, - .timer_channel = 4, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH1OUT, - .timer_index = 1, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH2OUT, - .timer_index = 1, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH3OUT, - .timer_index = 1, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH4OUT, - .timer_index = 1, - .timer_channel = 4, - .default_value = 1000, - } -}; diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index c7c25048a..37af26d52 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -59,9 +59,7 @@ # define GPIO_CAN_RX (1<<7) /**< CAN2 RX */ /** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. + * Device paths for things that support the GPIO ioctl protocol. */ # define PX4FMU_DEVICE_PATH "/dev/px4fmu" # define PX4IO_DEVICE_PATH "/dev/px4io" @@ -89,9 +87,7 @@ # define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */ /** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. + * Device paths for things that support the GPIO ioctl protocol. */ # define PX4FMU_DEVICE_PATH "/dev/px4fmu" # define PX4IO_DEVICE_PATH "/dev/px4io" diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index de8028b0f..f79b0b1a3 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ac3bdc132..039b496f4 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -58,7 +58,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 61a38b125..3bb9a7764 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -59,11 +59,10 @@ #include #include -#include - #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 844cc3884..d9801f88f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -59,12 +59,12 @@ #include #include -#include - #include #include #include +#include + #include "iirFilter.h" /* oddly, ERROR is not defined for c++ */ diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 397686e8b..c5f49fb36 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -59,8 +59,6 @@ #include #include -#include - #include #include @@ -70,6 +68,8 @@ #include #include +#include + /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7a2e22c01..b19a1a0e6 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -69,7 +69,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1f4a63cf3..4ad13fc04 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -59,10 +59,11 @@ #include #include +#include + #include #include #include -#include #include #include diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1fd6cb17e..0e27a382a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -63,7 +63,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b73f71572..a6f337486 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -59,15 +59,7 @@ #include #include -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) -# include -# define FMU_HAVE_PPM -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) -# include -# undef FMU_HAVE_PPM -#else -# error Unrecognised FMU board. -#endif +# include #include #include @@ -80,7 +72,7 @@ #include #include -#ifdef FMU_HAVE_PPM +#ifdef HRT_PPM_CHANNEL # include #endif @@ -455,7 +447,7 @@ PX4FMU::task_main() fds[1].fd = _t_armed; fds[1].events = POLLIN; -#ifdef FMU_HAVE_PPM +#ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; @@ -585,7 +577,7 @@ PX4FMU::task_main() } } -#ifdef FMU_HAVE_PPM +#ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 840b96f5b..236cb918d 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -61,7 +61,7 @@ #include #include -#include /* XXX should really not be hardcoding v2 here */ +#include #include diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index ec22a5e17..c7ce60b5e 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -56,12 +56,7 @@ #include "uploader.h" -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#include -#endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#include -#endif +#include // define for comms logging //#define UDEBUG diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index dae41d934..5c4fa4bb6 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -41,7 +41,6 @@ #include -#include #include #include @@ -60,6 +59,8 @@ #include #include +#include + #include "device/rgbled.h" #define LED_ONTIME 120 diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 1cc18afda..58529fb03 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include "chip.h" @@ -70,8 +70,6 @@ #include "stm32_gpio.h" #include "stm32_tim.h" -#ifdef HRT_TIMER - /* HRT configuration */ #if HRT_TIMER == 1 # define HRT_TIMER_BASE STM32_TIM1_BASE @@ -905,6 +903,3 @@ hrt_latency_update(void) /* catch-all at the end */ latency_counters[index]++; } - - -#endif /* HRT_TIMER */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 2284be84d..24eec52af 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -104,7 +104,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bd78f2638..9eb092a63 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,12 +42,7 @@ #include #include -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -# include -#endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 -# include -#endif +#include #include "protocol.h" diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index a2b0d8cae..96276b56a 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -43,11 +43,13 @@ #include #include #include -#include #include #include #include +#include +#include + #include "systemlib.h" __EXPORT extern void systemreset(void) { diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c index 49da51358..2aed80e01 100644 --- a/src/systemcmds/eeprom/eeprom.c +++ b/src/systemcmds/eeprom/eeprom.c @@ -55,7 +55,7 @@ #include #include -#include +#include #include "systemlib/systemlib.h" #include "systemlib/param/param.h" diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 4da238039..34405c496 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -52,7 +52,7 @@ #include -#include +#include #include "systemlib/systemlib.h" #include "systemlib/err.h" -- cgit v1.2.3 From ecc7bc5bca40e9d5e0a7271105dea9b3441af0a8 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 2 Aug 2013 23:11:04 -0700 Subject: Clean out unused trash from the NuttX configs. --- Makefile | 3 +- nuttx-configs/px4fmu-v1/Kconfig | 21 - nuttx-configs/px4fmu-v1/common/Make.defs | 184 ---- nuttx-configs/px4fmu-v1/common/ld.script | 149 ---- nuttx-configs/px4fmu-v1/nsh/Make.defs | 178 +++- nuttx-configs/px4fmu-v1/ostest/Make.defs | 122 --- nuttx-configs/px4fmu-v1/ostest/defconfig | 583 ------------- nuttx-configs/px4fmu-v1/ostest/setenv.sh | 75 -- nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld | 129 --- nuttx-configs/px4fmu-v1/scripts/ld.script | 58 +- nuttx-configs/px4fmu-v1/tools/px_mkfw.py | 110 --- nuttx-configs/px4fmu-v1/tools/px_uploader.py | 416 --------- nuttx-configs/px4fmu-v1/tools/upload.sh | 27 - nuttx-configs/px4fmu-v2/Kconfig | 7 - nuttx-configs/px4fmu-v2/common/Make.defs | 184 ---- nuttx-configs/px4fmu-v2/common/ld.script | 150 ---- nuttx-configs/px4fmu-v2/nsh/Make.defs | 178 +++- nuttx-configs/px4fmu-v2/nsh/defconfig.prev | 1067 ----------------------- nuttx-configs/px4fmu-v2/scripts/ld.script | 150 ++++ nuttx-configs/px4io-v1/Kconfig | 21 - nuttx-configs/px4io-v1/README.txt | 806 ----------------- nuttx-configs/px4io-v1/common/Make.defs | 171 ---- nuttx-configs/px4io-v1/common/ld.script | 129 --- nuttx-configs/px4io-v1/common/setenv.sh | 47 - nuttx-configs/px4io-v1/include/drv_i2c_device.h | 42 - nuttx-configs/px4io-v1/nsh/Make.defs | 165 +++- nuttx-configs/px4io-v1/scripts/ld.script | 129 +++ nuttx-configs/px4io-v1/src/README.txt | 1 - nuttx-configs/px4io-v1/src/drv_i2c_device.c | 618 ------------- nuttx-configs/px4io-v2/README.txt | 806 ----------------- nuttx-configs/px4io-v2/common/Make.defs | 175 ---- nuttx-configs/px4io-v2/common/ld.script | 129 --- nuttx-configs/px4io-v2/common/setenv.sh | 47 - nuttx-configs/px4io-v2/include/README.txt | 1 - nuttx-configs/px4io-v2/nsh/Make.defs | 169 +++- nuttx-configs/px4io-v2/scripts/ld.script | 129 +++ nuttx-configs/px4io-v2/src/README.txt | 1 - nuttx-configs/px4io-v2/test/Make.defs | 3 - nuttx-configs/px4io-v2/test/appconfig | 44 - nuttx-configs/px4io-v2/test/defconfig | 566 ------------ nuttx-configs/px4io-v2/test/setenv.sh | 47 - 41 files changed, 1138 insertions(+), 6899 deletions(-) delete mode 100644 nuttx-configs/px4fmu-v1/Kconfig delete mode 100644 nuttx-configs/px4fmu-v1/common/Make.defs delete mode 100644 nuttx-configs/px4fmu-v1/common/ld.script delete mode 100644 nuttx-configs/px4fmu-v1/ostest/Make.defs delete mode 100644 nuttx-configs/px4fmu-v1/ostest/defconfig delete mode 100755 nuttx-configs/px4fmu-v1/ostest/setenv.sh delete mode 100644 nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld delete mode 100755 nuttx-configs/px4fmu-v1/tools/px_mkfw.py delete mode 100755 nuttx-configs/px4fmu-v1/tools/px_uploader.py delete mode 100755 nuttx-configs/px4fmu-v1/tools/upload.sh delete mode 100644 nuttx-configs/px4fmu-v2/Kconfig delete mode 100644 nuttx-configs/px4fmu-v2/common/Make.defs delete mode 100644 nuttx-configs/px4fmu-v2/common/ld.script delete mode 100755 nuttx-configs/px4fmu-v2/nsh/defconfig.prev create mode 100644 nuttx-configs/px4fmu-v2/scripts/ld.script delete mode 100644 nuttx-configs/px4io-v1/Kconfig delete mode 100755 nuttx-configs/px4io-v1/README.txt delete mode 100644 nuttx-configs/px4io-v1/common/Make.defs delete mode 100755 nuttx-configs/px4io-v1/common/ld.script delete mode 100755 nuttx-configs/px4io-v1/common/setenv.sh delete mode 100644 nuttx-configs/px4io-v1/include/drv_i2c_device.h create mode 100755 nuttx-configs/px4io-v1/scripts/ld.script delete mode 100644 nuttx-configs/px4io-v1/src/README.txt delete mode 100644 nuttx-configs/px4io-v1/src/drv_i2c_device.c delete mode 100755 nuttx-configs/px4io-v2/README.txt delete mode 100644 nuttx-configs/px4io-v2/common/Make.defs delete mode 100755 nuttx-configs/px4io-v2/common/ld.script delete mode 100755 nuttx-configs/px4io-v2/common/setenv.sh delete mode 100755 nuttx-configs/px4io-v2/include/README.txt create mode 100755 nuttx-configs/px4io-v2/scripts/ld.script delete mode 100644 nuttx-configs/px4io-v2/src/README.txt delete mode 100644 nuttx-configs/px4io-v2/test/Make.defs delete mode 100644 nuttx-configs/px4io-v2/test/appconfig delete mode 100755 nuttx-configs/px4io-v2/test/defconfig delete mode 100755 nuttx-configs/px4io-v2/test/setenv.sh diff --git a/Makefile b/Makefile index 6f58858f0..d1a192fa2 100644 --- a/Makefile +++ b/Makefile @@ -148,12 +148,13 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/* .) + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export $(Q) mkdir -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ + $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) $(NUTTX_SRC): @$(ECHO) "" diff --git a/nuttx-configs/px4fmu-v1/Kconfig b/nuttx-configs/px4fmu-v1/Kconfig deleted file mode 100644 index edbafa06f..000000000 --- a/nuttx-configs/px4fmu-v1/Kconfig +++ /dev/null @@ -1,21 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4FMU_V1 - -config HRT_TIMER - bool "High resolution timer support" - default y - ---help--- - Enable high resolution timer for PPM capture and system clocks. - -config HRT_PPM - bool "PPM input capture" - default y - depends on HRT_TIMER - ---help--- - Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) - -endif diff --git a/nuttx-configs/px4fmu-v1/common/Make.defs b/nuttx-configs/px4fmu-v1/common/Make.defs deleted file mode 100644 index 756286ccb..000000000 --- a/nuttx-configs/px4fmu-v1/common/Make.defs +++ /dev/null @@ -1,184 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard - - -# enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 - -# pull in *just* libm from the toolchain ... this is grody -LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" -EXTRA_LIBS += $(LIBM) - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) \ - -Wno-psabi -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4fmu-v1/common/ld.script b/nuttx-configs/px4fmu-v1/common/ld.script deleted file mode 100644 index de8179e8d..000000000 --- a/nuttx-configs/px4fmu-v1/common/ld.script +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/common/ld.script - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -/* The STM32F405 has 1024Kb of FLASH beginning at address 0x0800:0000 and - * 192Kb of SRAM. SRAM is split up into three blocks: - * - * 1) 112Kb of SRAM beginning at address 0x2000:0000 - * 2) 16Kb of SRAM beginning at address 0x2001:c000 - * 3) 64Kb of TCM SRAM beginning at address 0x1000:0000 - * - * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 - * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address range. - * - * The first 0x4000 of flash is reserved for the bootloader. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K -} - -OUTPUT_ARCH(arm) - -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - - /* - * This is a hack to make the newlib libm __errno() call - * use the NuttX get_errno_ptr() function. - */ - __errno = get_errno_ptr; - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 81936334b..7b2ea703a 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -1,3 +1,179 @@ +############################################################################ +# configs/px4fmu-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4fmu-v1/common/Make.defs diff --git a/nuttx-configs/px4fmu-v1/ostest/Make.defs b/nuttx-configs/px4fmu-v1/ostest/Make.defs deleted file mode 100644 index 7b807abdb..000000000 --- a/nuttx-configs/px4fmu-v1/ostest/Make.defs +++ /dev/null @@ -1,122 +0,0 @@ -############################################################################ -# configs/stm32f4discovery/ostest/Make.defs -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -include ${TOPDIR}/.config -include ${TOPDIR}/tools/Config.mk -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" - MAXOPTIMIZATION = -O2 -else - # Linux/Cygwin-native toolchain - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) -endif - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(ARCROSSDEV)ar rcs -NM = $(ARCROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -ifeq ($(CONFIG_DEBUG_SYMBOLS),y) - ARCHOPTIMIZATION = -g -else - ARCHOPTIMIZATION = $(MAXOPTIMIZATION) -fno-strict-aliasing -fno-strength-reduce -fomit-frame-pointer -endif - -ARCHCFLAGS = -fno-builtin -ARCHCXXFLAGS = -fno-builtin -fno-exceptions -fno-rtti -ARCHWARNINGS = -Wall -Wstrict-prototypes -Wshadow -ARCHWARNINGSXX = -Wall -Wshadow -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -CFLAGS = $(ARCHCFLAGS) $(ARCHWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat-pcrel.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -ifneq ($(CROSSDEV),arm-nuttx-elf-) - LDFLAGS += -nostartfiles -nodefaultlibs -endif -ifeq ($(CONFIG_DEBUG_SYMBOLS),y) - LDFLAGS += -g -endif - - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = -ifeq ($(CONFIG_HOST_WINDOWS),y) - HOSTEXEEXT = .exe -else - HOSTEXEEXT = -endif - -ifeq ($(WINTOOL),y) - # Windows-native host tools - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh -else - # Linux/Cygwin-native host tools - MKDEP = $(TOPDIR)/tools/mkdeps$(HOSTEXEEXT) -endif - diff --git a/nuttx-configs/px4fmu-v1/ostest/defconfig b/nuttx-configs/px4fmu-v1/ostest/defconfig deleted file mode 100644 index c7fb6b2a5..000000000 --- a/nuttx-configs/px4fmu-v1/ostest/defconfig +++ /dev/null @@ -1,583 +0,0 @@ -# -# Automatically generated file; DO NOT EDIT. -# Nuttx/ Configuration -# -CONFIG_NUTTX_NEWCONFIG=y - -# -# Build Setup -# -# CONFIG_EXPERIMENTAL is not set -# CONFIG_HOST_LINUX is not set -# CONFIG_HOST_OSX is not set -CONFIG_HOST_WINDOWS=y -# CONFIG_HOST_OTHER is not set -# CONFIG_WINDOWS_NATIVE is not set -CONFIG_WINDOWS_CYGWIN=y -# CONFIG_WINDOWS_MSYS is not set -# CONFIG_WINDOWS_OTHER is not set -# CONFIG_WINDOWS_MKLINK is not set - -# -# Build Configuration -# -# CONFIG_APPS_DIR="../apps" -# CONFIG_BUILD_2PASS is not set - -# -# Binary Output Formats -# -# CONFIG_RRLOAD_BINARY is not set -CONFIG_INTELHEX_BINARY=y -# CONFIG_MOTOROLA_SREC is not set -CONFIG_RAW_BINARY=y - -# -# Customize Header Files -# -# CONFIG_ARCH_STDBOOL_H is not set -# CONFIG_ARCH_MATH_H is not set -# CONFIG_ARCH_FLOAT_H is not set -# CONFIG_ARCH_STDARG_H is not set - -# -# Debug Options -# -# CONFIG_DEBUG is not set -# CONFIG_DEBUG_SYMBOLS is not set - -# -# System Type -# -# CONFIG_ARCH_8051 is not set -CONFIG_ARCH_ARM=y -# CONFIG_ARCH_AVR is not set -# CONFIG_ARCH_HC is not set -# CONFIG_ARCH_MIPS is not set -# CONFIG_ARCH_RGMP is not set -# CONFIG_ARCH_SH is not set -# CONFIG_ARCH_SIM is not set -# CONFIG_ARCH_X86 is not set -# CONFIG_ARCH_Z16 is not set -# CONFIG_ARCH_Z80 is not set -CONFIG_ARCH="arm" - -# -# ARM Options -# -# CONFIG_ARCH_CHIP_C5471 is not set -# CONFIG_ARCH_CHIP_CALYPSO is not set -# CONFIG_ARCH_CHIP_DM320 is not set -# CONFIG_ARCH_CHIP_IMX is not set -# CONFIG_ARCH_CHIP_KINETIS is not set -# CONFIG_ARCH_CHIP_LM3S is not set -# CONFIG_ARCH_CHIP_LPC17XX is not set -# CONFIG_ARCH_CHIP_LPC214X is not set -# CONFIG_ARCH_CHIP_LPC2378 is not set -# CONFIG_ARCH_CHIP_LPC31XX is not set -# CONFIG_ARCH_CHIP_LPC43XX is not set -# CONFIG_ARCH_CHIP_SAM3U is not set -CONFIG_ARCH_CHIP_STM32=y -# CONFIG_ARCH_CHIP_STR71X is not set -CONFIG_ARCH_CORTEXM4=y -CONFIG_ARCH_FAMILY="armv7-m" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_HAVE_CMNVECTOR=y -# CONFIG_ARMV7M_CMNVECTOR is not set -# CONFIG_ARCH_FPU is not set -CONFIG_ARCH_HAVE_MPU=y -# CONFIG_ARMV7M_MPU is not set -CONFIG_ARCH_IRQPRIO=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -# CONFIG_ARCH_CALIBRATION is not set -# CONFIG_SERIAL_TERMIOS is not set - -# -# STM32 Configuration Options -# -# CONFIG_ARCH_CHIP_STM32F100C8 is not set -# CONFIG_ARCH_CHIP_STM32F100CB is not set -# CONFIG_ARCH_CHIP_STM32F100R8 is not set -# CONFIG_ARCH_CHIP_STM32F100RB is not set -# CONFIG_ARCH_CHIP_STM32F100RC is not set -# CONFIG_ARCH_CHIP_STM32F100RD is not set -# CONFIG_ARCH_CHIP_STM32F100RE is not set -# CONFIG_ARCH_CHIP_STM32F100V8 is not set -# CONFIG_ARCH_CHIP_STM32F100VB is not set -# CONFIG_ARCH_CHIP_STM32F100VC is not set -# CONFIG_ARCH_CHIP_STM32F100VD is not set -# CONFIG_ARCH_CHIP_STM32F100VE is not set -# CONFIG_ARCH_CHIP_STM32F103RET6 is not set -# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set -# CONFIG_ARCH_CHIP_STM32F103VET6 is not set -# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set -# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set -# CONFIG_ARCH_CHIP_STM32F107VC is not set -# CONFIG_ARCH_CHIP_STM32F207IG is not set -# CONFIG_ARCH_CHIP_STM32F405RG is not set -# CONFIG_ARCH_CHIP_STM32F405VG is not set -# CONFIG_ARCH_CHIP_STM32F405ZG is not set -# CONFIG_ARCH_CHIP_STM32F407VE is not set -CONFIG_ARCH_CHIP_STM32F407VG=y -# CONFIG_ARCH_CHIP_STM32F407ZE is not set -# CONFIG_ARCH_CHIP_STM32F407ZG is not set -# CONFIG_ARCH_CHIP_STM32F407IE is not set -# CONFIG_ARCH_CHIP_STM32F407IG is not set -CONFIG_STM32_STM32F40XX=y -# CONFIG_STM32_CODESOURCERYW is not set -CONFIG_STM32_CODESOURCERYL=y -# CONFIG_STM32_ATOLLIC_LITE is not set -# CONFIG_STM32_ATOLLIC_PRO is not set -# CONFIG_STM32_DEVKITARM is not set -# CONFIG_STM32_RAISONANCE is not set -# CONFIG_STM32_BUILDROOT is not set -# CONFIG_STM32_DFU is not set - -# -# STM32 Peripheral Support -# -# CONFIG_STM32_ADC1 is not set -# CONFIG_STM32_ADC2 is not set -# CONFIG_STM32_ADC3 is not set -# CONFIG_STM32_BKPSRAM is not set -# CONFIG_STM32_CAN1 is not set -# CONFIG_STM32_CAN2 is not set -# CONFIG_STM32_CCMDATARAM is not set -# CONFIG_STM32_CRC is not set -# CONFIG_STM32_CRYP is not set -# CONFIG_STM32_DMA1 is not set -# CONFIG_STM32_DMA2 is not set -# CONFIG_STM32_DAC1 is not set -# CONFIG_STM32_DAC2 is not set -# CONFIG_STM32_DCMI is not set -# CONFIG_STM32_ETHMAC is not set -# CONFIG_STM32_FSMC is not set -# CONFIG_STM32_HASH is not set -# CONFIG_STM32_I2C1 is not set -# CONFIG_STM32_I2C2 is not set -# CONFIG_STM32_I2C3 is not set -# CONFIG_STM32_IWDG is not set -# CONFIG_STM32_OTGFS is not set -# CONFIG_STM32_OTGHS is not set -# CONFIG_STM32_PWR is not set -# CONFIG_STM32_RNG is not set -# CONFIG_STM32_SDIO is not set -# CONFIG_STM32_SPI1 is not set -# CONFIG_STM32_SPI2 is not set -# CONFIG_STM32_SPI3 is not set -CONFIG_STM32_SYSCFG=y -# CONFIG_STM32_TIM1 is not set -# CONFIG_STM32_TIM2 is not set -# CONFIG_STM32_TIM3 is not set -# CONFIG_STM32_TIM4 is not set -# CONFIG_STM32_TIM5 is not set -# CONFIG_STM32_TIM6 is not set -# CONFIG_STM32_TIM7 is not set -# CONFIG_STM32_TIM8 is not set -# CONFIG_STM32_TIM9 is not set -# CONFIG_STM32_TIM10 is not set -# CONFIG_STM32_TIM11 is not set -# CONFIG_STM32_TIM12 is not set -# CONFIG_STM32_TIM13 is not set -# CONFIG_STM32_TIM14 is not set -# CONFIG_STM32_USART1 is not set -CONFIG_STM32_USART2=y -# CONFIG_STM32_USART3 is not set -# CONFIG_STM32_UART4 is not set -# CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set -# CONFIG_STM32_WWDG is not set - -# -# Alternate Pin Mapping -# -# CONFIG_STM32_JTAG_DISABLE is not set -# CONFIG_STM32_JTAG_FULL_ENABLE is not set -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set -CONFIG_STM32_JTAG_SW_ENABLE=y -# CONFIG_STM32_FORCEPOWER is not set -# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -# CONFIG_STM32_CCMEXCLUDE is not set - -# -# USB Host Configuration -# - -# -# Architecture Options -# -# CONFIG_ARCH_NOINTC is not set -# CONFIG_ARCH_DMA is not set -CONFIG_ARCH_STACKDUMP=y - -# -# Board Settings -# -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_SIZE=114688 -CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=0 - -# -# Boot options -# -# CONFIG_BOOT_RUNFROMEXTSRAM is not set -CONFIG_BOOT_RUNFROMFLASH=y -# CONFIG_BOOT_RUNFROMISRAM is not set -# CONFIG_BOOT_RUNFROMSDRAM is not set -# CONFIG_BOOT_COPYTORAM is not set - -# -# Board Selection -# -CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y -# CONFIG_ARCH_BOARD_CUSTOM is not set -CONFIG_ARCH_BOARD="stm32f4discovery" - -# -# Common Board Options -# -CONFIG_ARCH_HAVE_LEDS=y -CONFIG_ARCH_LEDS=y -CONFIG_ARCH_HAVE_BUTTONS=y -# CONFIG_ARCH_BUTTONS is not set -CONFIG_ARCH_HAVE_IRQBUTTONS=y - -# -# Board-Specific Options -# - -# -# RTOS Features -# -CONFIG_MSEC_PER_TICK=10 -CONFIG_RR_INTERVAL=200 -# CONFIG_SCHED_INSTRUMENTATION is not set -CONFIG_TASK_NAME_SIZE=0 -# CONFIG_JULIAN_TIME is not set -CONFIG_START_YEAR=2009 -CONFIG_START_MONTH=9 -CONFIG_START_DAY=21 -CONFIG_DEV_CONSOLE=y -# CONFIG_MUTEX_TYPES is not set -# CONFIG_PRIORITY_INHERITANCE is not set -# CONFIG_FDCLONE_DISABLE is not set -# CONFIG_FDCLONE_STDIO is not set -CONFIG_SDCLONE_DISABLE=y -# CONFIG_SCHED_WORKQUEUE is not set -# CONFIG_SCHED_WAITPID is not set -# CONFIG_SCHED_ATEXIT is not set -# CONFIG_SCHED_ONEXIT is not set -CONFIG_USER_ENTRYPOINT="ostest_main" -CONFIG_DISABLE_OS_API=y -# CONFIG_DISABLE_CLOCK is not set -# CONFIG_DISABLE_POSIX_TIMERS is not set -# CONFIG_DISABLE_PTHREAD is not set -# CONFIG_DISABLE_SIGNALS is not set -# CONFIG_DISABLE_MQUEUE is not set -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Sizes of configurable things (0 disables) -# -CONFIG_MAX_TASKS=16 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=8 -CONFIG_NFILE_STREAMS=8 -CONFIG_NAME_MAX=32 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=4 -CONFIG_PREALLOC_TIMERS=4 - -# -# Stack and heap information -# -# CONFIG_CUSTOM_STACK is not set -CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=2048 -CONFIG_PTHREAD_STACK_MIN=256 -CONFIG_PTHREAD_STACK_DEFAULT=2048 - -# -# Device Drivers -# -CONFIG_DEV_NULL=y -# CONFIG_DEV_ZERO is not set -# CONFIG_LOOP is not set -# CONFIG_RAMDISK is not set -# CONFIG_CAN is not set -# CONFIG_PWM is not set -# CONFIG_I2C is not set -CONFIG_ARCH_HAVE_I2CRESET=y -# CONFIG_SPI is not set -# CONFIG_RTC is not set -# CONFIG_WATCHDOG is not set -# CONFIG_ANALOG is not set -# CONFIG_BCH is not set -# CONFIG_INPUT is not set -# CONFIG_LCD is not set -# CONFIG_MMCSD is not set -# CONFIG_MTD is not set -# CONFIG_PIPES is not set -# CONFIG_PM is not set -# CONFIG_POWER is not set -# CONFIG_SENSORS is not set -# CONFIG_SERCOMM_CONSOLE is not set -CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y -# CONFIG_16550_UART is not set -CONFIG_ARCH_HAVE_USART2=y -CONFIG_MCU_SERIAL=y -CONFIG_STANDARD_SERIAL=y -CONFIG_USART2_SERIAL_CONSOLE=y -# CONFIG_NO_SERIAL_CONSOLE is not set - -# -# USART2 Configuration -# -CONFIG_USART2_RXBUFSIZE=128 -CONFIG_USART2_TXBUFSIZE=128 -CONFIG_USART2_BAUD=115200 -CONFIG_USART2_BITS=8 -CONFIG_USART2_PARITY=0 -CONFIG_USART2_2STOP=0 -# CONFIG_USBDEV is not set -# CONFIG_USBHOST is not set -# CONFIG_WIRELESS is not set - -# -# System Logging Device Options -# - -# -# System Logging -# -# CONFIG_RAMLOG is not set - -# -# Networking Support -# -# CONFIG_NET is not set - -# -# File Systems -# - -# -# File system configuration -# -# CONFIG_FS_RAMMAP is not set - -# -# System Logging -# -# CONFIG_SYSLOG is not set - -# -# Graphics Support -# -# CONFIG_NX is not set - -# -# Memory Management -# -# CONFIG_MM_SMALL is not set -CONFIG_MM_REGIONS=2 -# CONFIG_GRAN is not set - -# -# Binary Formats -# -# CONFIG_BINFMT_DISABLE is not set -# CONFIG_NXFLAT is not set -# CONFIG_ELF is not set -CONFIG_SYMTAB_ORDEREDBYNAME=y - -# -# Library Routines -# -CONFIG_STDIO_BUFFER_SIZE=64 -CONFIG_STDIO_LINEBUFFER=y -CONFIG_NUNGET_CHARS=2 -# CONFIG_LIBM is not set -# CONFIG_NOPRINTF_FIELDWIDTH is not set -# CONFIG_LIBC_FLOATINGPOINT is not set -# CONFIG_EOL_IS_CR is not set -# CONFIG_EOL_IS_LF is not set -# CONFIG_EOL_IS_BOTH_CRLF is not set -CONFIG_EOL_IS_EITHER_CRLF=y -# CONFIG_LIBC_STRERROR is not set -# CONFIG_LIBC_PERROR_STDOUT is not set -CONFIG_ARCH_LOWPUTC=y -CONFIG_LIB_SENDFILE_BUFSIZE=512 -# CONFIG_ARCH_ROMGETC is not set -# CONFIG_ARCH_OPTIMIZED_FUNCTIONS is not set - -# -# Basic CXX Support -# -# CONFIG_HAVE_CXX is not set - -# -# Application Configuration -# - -# -# Named Applications -# -# CONFIG_BUILTIN is not set - -# -# Examples -# -# CONFIG_EXAMPLES_BUTTONS is not set -# CONFIG_EXAMPLES_CAN is not set -# CONFIG_EXAMPLES_CDCACM is not set -# CONFIG_EXAMPLES_COMPOSITE is not set -# CONFIG_EXAMPLES_DHCPD is not set -# CONFIG_EXAMPLES_ELF is not set -# CONFIG_EXAMPLES_FTPC is not set -# CONFIG_EXAMPLES_FTPD is not set -# CONFIG_EXAMPLES_HELLO is not set -# CONFIG_EXAMPLES_HELLOXX is not set -# CONFIG_EXAMPLES_JSON is not set -# CONFIG_EXAMPLES_HIDKBD is not set -# CONFIG_EXAMPLES_IGMP is not set -# CONFIG_EXAMPLES_LCDRW is not set -# CONFIG_EXAMPLES_MM is not set -# CONFIG_EXAMPLES_MOUNT is not set -# CONFIG_EXAMPLES_MODBUS is not set -# CONFIG_EXAMPLES_NETTEST is not set -# CONFIG_EXAMPLES_NSH is not set -# CONFIG_EXAMPLES_NULL is not set -# CONFIG_EXAMPLES_NX is not set -# CONFIG_EXAMPLES_NXCONSOLE is not set -# CONFIG_EXAMPLES_NXFFS is not set -# CONFIG_EXAMPLES_NXFLAT is not set -# CONFIG_EXAMPLES_NXHELLO is not set -# CONFIG_EXAMPLES_NXIMAGE is not set -# CONFIG_EXAMPLES_NXLINES is not set -# CONFIG_EXAMPLES_NXTEXT is not set -CONFIG_EXAMPLES_OSTEST=y -# CONFIG_EXAMPLES_OSTEST_BUILTIN is not set -CONFIG_EXAMPLES_OSTEST_LOOPS=1 -CONFIG_EXAMPLES_OSTEST_STACKSIZE=2048 -CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS=3 -CONFIG_EXAMPLES_OSTEST_RR_RANGE=10000 -CONFIG_EXAMPLES_OSTEST_RR_RUNS=10 -# CONFIG_EXAMPLES_PASHELLO is not set -# CONFIG_EXAMPLES_PIPE is not set -# CONFIG_EXAMPLES_POLL is not set -# CONFIG_EXAMPLES_QENCODER is not set -# CONFIG_EXAMPLES_RGMP is not set -# CONFIG_EXAMPLES_ROMFS is not set -# CONFIG_EXAMPLES_SENDMAIL is not set -# CONFIG_EXAMPLES_SERLOOP is not set -# CONFIG_EXAMPLES_TELNETD is not set -# CONFIG_EXAMPLES_THTTPD is not set -# CONFIG_EXAMPLES_TIFF is not set -# CONFIG_EXAMPLES_TOUCHSCREEN is not set -# CONFIG_EXAMPLES_UDP is not set -# CONFIG_EXAMPLES_UIP is not set -# CONFIG_EXAMPLES_USBSERIAL is not set -# CONFIG_EXAMPLES_USBMSC is not set -# CONFIG_EXAMPLES_USBTERM is not set -# CONFIG_EXAMPLES_WATCHDOG is not set -# CONFIG_EXAMPLES_WLAN is not set - -# -# Interpreters -# - -# -# Interpreters -# -# CONFIG_INTERPRETERS_FICL is not set -# CONFIG_INTERPRETERS_PCODE is not set - -# -# Network Utilities -# - -# -# Networking Utilities -# -# CONFIG_NETUTILS_CODECS is not set -# CONFIG_NETUTILS_DHCPC is not set -# CONFIG_NETUTILS_DHCPD is not set -# CONFIG_NETUTILS_FTPC is not set -# CONFIG_NETUTILS_FTPD is not set -# CONFIG_NETUTILS_JSON is not set -# CONFIG_NETUTILS_RESOLV is not set -# CONFIG_NETUTILS_SMTP is not set -# CONFIG_NETUTILS_TELNETD is not set -# CONFIG_NETUTILS_TFTPC is not set -# CONFIG_NETUTILS_THTTPD is not set -# CONFIG_NETUTILS_UIPLIB is not set -# CONFIG_NETUTILS_WEBCLIENT is not set - -# -# ModBus -# - -# -# FreeModbus -# -# CONFIG_MODBUS is not set - -# -# NSH Library -# -# CONFIG_NSH_LIBRARY is not set - -# -# NxWidgets/NxWM -# - -# -# System NSH Add-Ons -# - -# -# Custom Free Memory Command -# -# CONFIG_SYSTEM_FREE is not set - -# -# I2C tool -# - -# -# FLASH Program Installation -# -# CONFIG_SYSTEM_INSTALL is not set - -# -# readline() -# -# CONFIG_SYSTEM_READLINE is not set - -# -# Power Off -# -# CONFIG_SYSTEM_POWEROFF is not set - -# -# RAMTRON -# -# CONFIG_SYSTEM_RAMTRON is not set - -# -# SD Card -# -# CONFIG_SYSTEM_SDCARD is not set - -# -# Sysinfo -# -# CONFIG_SYSTEM_SYSINFO is not set diff --git a/nuttx-configs/px4fmu-v1/ostest/setenv.sh b/nuttx-configs/px4fmu-v1/ostest/setenv.sh deleted file mode 100755 index a67fdc5a8..000000000 --- a/nuttx-configs/px4fmu-v1/ostest/setenv.sh +++ /dev/null @@ -1,75 +0,0 @@ -#!/bin/bash -# configs/stm32f4discovery/ostest/setenv.sh -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$_" = "$0" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -WD=`pwd` -if [ ! -x "setenv.sh" ]; then - echo "This script must be executed from the top-level NuttX build directory" - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then - export PATH_ORIG="${PATH}" -fi - -# This is the Cygwin path to the location where I installed the RIDE -# toolchain under windows. You will also have to edit this if you install -# the RIDE toolchain in any other location -#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" - -# This is the Cygwin path to the location where I installed the CodeSourcery -# toolchain under windows. You will also have to edit this if you install -# the CodeSourcery toolchain in any other location -export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" - -# These are the Cygwin paths to the locations where I installed the Atollic -# toolchain under windows. You will also have to edit this if you install -# the Atollic toolchain in any other location. /usr/bin is added before -# the Atollic bin path because there is are binaries named gcc.exe and g++.exe -# at those locations as well. -#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" -#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" - -# This is the Cygwin path to the location where I build the buildroot -# toolchain. -#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" - -# Add the path to the toolchain to the PATH varialble -export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld b/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld deleted file mode 100644 index 1f29f02f5..000000000 --- a/nuttx-configs/px4fmu-v1/scripts/gnu-elf.ld +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * configs/stm32f4discovery/scripts/gnu-elf.ld - * - * Copyright (C) 2012 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -SECTIONS -{ - .text 0x00000000 : - { - _stext = . ; - *(.text) - *(.text.*) - *(.gnu.warning) - *(.stub) - *(.glue_7) - *(.glue_7t) - *(.jcr) - - /* C++ support: The .init and .fini sections contain specific logic - * to manage static constructors and destructors. - */ - - *(.gnu.linkonce.t.*) - *(.init) /* Old ABI */ - *(.fini) /* Old ABI */ - _etext = . ; - } - - .rodata : - { - _srodata = . ; - *(.rodata) - *(.rodata1) - *(.rodata.*) - *(.gnu.linkonce.r*) - _erodata = . ; - } - - .data : - { - _sdata = . ; - *(.data) - *(.data1) - *(.data.*) - *(.gnu.linkonce.d*) - _edata = . ; - } - - /* C++ support. For each global and static local C++ object, - * GCC creates a small subroutine to construct the object. Pointers - * to these routines (not the routines themselves) are stored as - * simple, linear arrays in the .ctors section of the object file. - * Similarly, pointers to global/static destructor routines are - * stored in .dtors. - */ - - .ctors : - { - _sctors = . ; - *(.ctors) /* Old ABI: Unallocated */ - *(.init_array) /* New ABI: Allocated */ - _edtors = . ; - } - - .dtors : - { - _sdtors = . ; - *(.dtors) /* Old ABI: Unallocated */ - *(.fini_array) /* New ABI: Allocated */ - _edtors = . ; - } - - .bss : - { - _sbss = . ; - *(.bss) - *(.bss.*) - *(.sbss) - *(.sbss.*) - *(.gnu.linkonce.b*) - *(COMMON) - _ebss = . ; - } - - /* Stabs debugging sections. */ - - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script index f6560743b..ced5b21b7 100644 --- a/nuttx-configs/px4fmu-v1/scripts/ld.script +++ b/nuttx-configs/px4fmu-v1/scripts/ld.script @@ -1,7 +1,7 @@ /**************************************************************************** - * configs/stm32f4discovery/scripts/ld.script + * configs/px4fmu-v1/scripts/ld.script * - * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2011 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -42,41 +42,67 @@ * * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address - * range. + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. */ MEMORY { - flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 112K + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } OUTPUT_ARCH(arm) -EXTERN(_vectors) -ENTRY(_stext) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) - *(.text .text.*) + *(.text .text.*) *(.fixup) *(.gnu.warning) - *(.rodata .rodata.*) + *(.rodata .rodata.*) *(.gnu.linkonce.t.*) - *(.glue_7) - *(.glue_7t) *(.got) *(.gcc_except_table) *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; } > flash - .init_section : { - _sinit = ABSOLUTE(.); - *(.init_array .init_array.*) - _einit = ABSOLUTE(.); + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); } > flash .ARM.extab : { diff --git a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py b/nuttx-configs/px4fmu-v1/tools/px_mkfw.py deleted file mode 100755 index 9f4ddad62..000000000 --- a/nuttx-configs/px4fmu-v1/tools/px_mkfw.py +++ /dev/null @@ -1,110 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# 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. -# -############################################################################ - -# -# PX4 firmware image generator -# -# The PX4 firmware file is a JSON-encoded Python object, containing -# metadata fields and a zlib-compressed base64-encoded firmware image. -# - -import sys -import argparse -import json -import base64 -import zlib -import time -import subprocess - -# -# Construct a basic firmware description -# -def mkdesc(): - proto = {} - proto['magic'] = "PX4FWv1" - proto['board_id'] = 0 - proto['board_revision'] = 0 - proto['version'] = "" - proto['summary'] = "" - proto['description'] = "" - proto['git_identity'] = "" - proto['build_time'] = 0 - proto['image'] = base64.b64encode(bytearray()) - proto['image_size'] = 0 - return proto - -# Parse commandline -parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.") -parser.add_argument("--prototype", action="store", help="read a prototype description from a file") -parser.add_argument("--board_id", action="store", help="set the board ID required") -parser.add_argument("--board_revision", action="store", help="set the board revision required") -parser.add_argument("--version", action="store", help="set a version string") -parser.add_argument("--summary", action="store", help="set a brief description") -parser.add_argument("--description", action="store", help="set a longer description") -parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") -parser.add_argument("--image", action="store", help="the firmware image") -args = parser.parse_args() - -# Fetch the firmware descriptor prototype if specified -if args.prototype != None: - f = open(args.prototype,"r") - desc = json.load(f) - f.close() -else: - desc = mkdesc() - -desc['build_time'] = int(time.time()) - -if args.board_id != None: - desc['board_id'] = int(args.board_id) -if args.board_revision != None: - desc['board_revision'] = int(args.board_revision) -if args.version != None: - desc['version'] = str(args.version) -if args.summary != None: - desc['summary'] = str(args.summary) -if args.description != None: - desc['description'] = str(args.description) -if args.git_identity != None: - cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) - p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout - desc['git_identity'] = p.read().strip() - p.close() -if args.image != None: - f = open(args.image, "rb") - bytes = f.read() - desc['image_size'] = len(bytes) - desc['image'] = base64.b64encode(zlib.compress(bytes,9)) - -print json.dumps(desc, indent=4) diff --git a/nuttx-configs/px4fmu-v1/tools/px_uploader.py b/nuttx-configs/px4fmu-v1/tools/px_uploader.py deleted file mode 100755 index 3b23f4f83..000000000 --- a/nuttx-configs/px4fmu-v1/tools/px_uploader.py +++ /dev/null @@ -1,416 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# 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. -# -############################################################################ - -# -# Serial firmware uploader for the PX4FMU bootloader -# -# The PX4 firmware file is a JSON-encoded Python object, containing -# metadata fields and a zlib-compressed base64-encoded firmware image. -# -# The uploader uses the following fields from the firmware file: -# -# image -# The firmware that will be uploaded. -# image_size -# The size of the firmware in bytes. -# board_id -# The board for which the firmware is intended. -# board_revision -# Currently only used for informational purposes. -# - -import sys -import argparse -import binascii -import serial -import os -import struct -import json -import zlib -import base64 -import time -import array - -from sys import platform as _platform - -class firmware(object): - '''Loads a firmware file''' - - desc = {} - image = bytes() - crctab = array.array('I', [ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ]) - crcpad = bytearray('\xff\xff\xff\xff') - - def __init__(self, path): - - # read the file - f = open(path, "r") - self.desc = json.load(f) - f.close() - - self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image']))) - - # pad image to 4-byte length - while ((len(self.image) % 4) != 0): - self.image.append('\xff') - - def property(self, propname): - return self.desc[propname] - - def __crc32(self, bytes, state): - for byte in bytes: - index = (state ^ byte) & 0xff - state = self.crctab[index] ^ (state >> 8) - return state - - def crc(self, padlen): - state = self.__crc32(self.image, int(0)) - for i in range(len(self.image), (padlen - 1), 4): - state = self.__crc32(self.crcpad, state) - return state - -class uploader(object): - '''Uploads a firmware file to the PX FMU bootloader''' - - # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) - - # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ - - # command bytes - NOP = chr(0x00) # guaranteed to be discarded by the bootloader - GET_SYNC = chr(0x21) - GET_DEVICE = chr(0x22) - CHIP_ERASE = chr(0x23) - CHIP_VERIFY = chr(0x24) # rev2 only - PROG_MULTI = chr(0x27) - READ_MULTI = chr(0x28) # rev2 only - GET_CRC = chr(0x29) # rev3+ - REBOOT = chr(0x30) - - INFO_BL_REV = chr(1) # bootloader protocol revision - BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol - INFO_BOARD_ID = chr(2) # board type - INFO_BOARD_REV = chr(3) # board revision - INFO_FLASH_SIZE = chr(4) # max firmware size in bytes - - PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 - READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 - - def __init__(self, portname, baudrate): - # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=0.25) - - def close(self): - if self.port is not None: - self.port.close() - - def __send(self, c): -# print("send " + binascii.hexlify(c)) - self.port.write(str(c)) - - def __recv(self, count = 1): - c = self.port.read(count) - if len(c) < 1: - raise RuntimeError("timeout waiting for data") -# print("recv " + binascii.hexlify(c)) - return c - - def __recv_int(self): - raw = self.__recv(4) - val = struct.unpack("= 3: - self.__getSync() - - # split a sequence into a list of size-constrained pieces - def __split_len(self, seq, length): - return [seq[i:i+length] for i in range(0, len(seq), length)] - - # upload code - def __program(self, fw): - code = fw.image - groups = self.__split_len(code, uploader.PROG_MULTI_MAX) - for bytes in groups: - self.__program_multi(bytes) - - # verify code - def __verify_v2(self, fw): - self.__send(uploader.CHIP_VERIFY - + uploader.EOC) - self.__getSync() - code = fw.image - groups = self.__split_len(code, uploader.READ_MULTI_MAX) - for bytes in groups: - if (not self.__verify_multi(bytes)): - raise RuntimeError("Verification failed") - - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) - self.__send(uploader.GET_CRC - + uploader.EOC) - report_crc = self.__recv_int() - self.__getSync() - if report_crc != expect_crc: - print("Expected 0x%x" % expect_crc) - print("Got 0x%x" % report_crc) - raise RuntimeError("Program CRC failed") - - # get basic data about the board - def identify(self): - # make sure we are in sync before starting - self.__sync() - - # get the bootloader protocol ID first - self.bl_rev = self.__getInfo(uploader.INFO_BL_REV) - if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX): - print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV) - raise RuntimeError("Bootloader protocol mismatch") - - self.board_type = self.__getInfo(uploader.INFO_BOARD_ID) - self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) - self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) - - # upload the firmware - def upload(self, fw): - # Make sure we are doing the right thing - if self.board_type != fw.property('board_id'): - raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).") - if self.fw_maxsize < fw.property('image_size'): - raise RuntimeError("Firmware image is too large for this board") - - print("erase...") - self.__erase() - - print("program...") - self.__program(fw) - - print("verify...") - if self.bl_rev == 2: - self.__verify_v2(fw) - else: - self.__verify_v3(fw) - - print("done, rebooting.") - self.__reboot() - self.port.close() - - -# Parse commandline arguments -parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") -parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") -parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") -parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") -args = parser.parse_args() - -# Load the firmware file -fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) - -# Spin waiting for a device to show up -while True: - for port in args.port.split(","): - - #print("Trying %s" % port) - - # create an uploader attached to the port - try: - if "linux" in _platform: - # Linux, don't open Mac OS and Win ports - if not "COM" in port and not "tty.usb" in port: - up = uploader(port, args.baud) - elif "darwin" in _platform: - # OS X, don't open Windows and Linux ports - if not "COM" in port and not "ACM" in port: - up = uploader(port, args.baud) - elif "win" in _platform: - # Windows, don't open POSIX ports - if not "/" in port: - up = uploader(port, args.baud) - except: - # open failed, rate-limit our attempts - time.sleep(0.05) - - # and loop to the next port - continue - - # port is open, try talking to it - try: - # identify the bootloader - up.identify() - print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - - except: - # most probably a timeout talking to the port, no bootloader - continue - - try: - # ok, we have a bootloader, try flashing it - up.upload(fw) - - except RuntimeError as ex: - - # print the error - print("ERROR: %s" % ex.args) - - finally: - # always close the port - up.close() - - # we could loop here if we wanted to wait for more boards... - sys.exit(0) diff --git a/nuttx-configs/px4fmu-v1/tools/upload.sh b/nuttx-configs/px4fmu-v1/tools/upload.sh deleted file mode 100755 index 4e6597b3a..000000000 --- a/nuttx-configs/px4fmu-v1/tools/upload.sh +++ /dev/null @@ -1,27 +0,0 @@ -#!/bin/sh -# -# Wrapper to upload a PX4 firmware binary -# -TOOLS=`dirname $0` -MKFW=${TOOLS}/px_mkfw.py -UPLOAD=${TOOLS}/px_uploader.py - -BINARY=nuttx.bin -PAYLOAD=nuttx.px4 -PORTS="/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" - -function abort() { - echo "ABORT: $*" - exit 1 -} - -if [ ! -f ${MKFW} -o ! -f ${UPLOAD} ]; then - abort "Missing tools ${MKFW} and/or ${UPLOAD}" -fi -if [ ! -f ${BINARY} ]; then - abort "Missing nuttx binary in current directory." -fi - -rm -f ${PAYLOAD} -${MKFW} --board_id 5 --image ${BINARY} > ${PAYLOAD} -${UPLOAD} --port ${PORTS} ${PAYLOAD} diff --git a/nuttx-configs/px4fmu-v2/Kconfig b/nuttx-configs/px4fmu-v2/Kconfig deleted file mode 100644 index 069b25f9e..000000000 --- a/nuttx-configs/px4fmu-v2/Kconfig +++ /dev/null @@ -1,7 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4FMU_V2 -endif diff --git a/nuttx-configs/px4fmu-v2/common/Make.defs b/nuttx-configs/px4fmu-v2/common/Make.defs deleted file mode 100644 index be387dce1..000000000 --- a/nuttx-configs/px4fmu-v2/common/Make.defs +++ /dev/null @@ -1,184 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMUV2 -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m4 \ - -mthumb \ - -march=armv7e-m \ - -mfpu=fpv4-sp-d16 \ - -mfloat-abi=hard - - -# enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 - -# pull in *just* libm from the toolchain ... this is grody -LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" -EXTRA_LIBS += $(LIBM) - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) \ - -Wno-psabi -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4fmu-v2/common/ld.script b/nuttx-configs/px4fmu-v2/common/ld.script deleted file mode 100644 index 1be39fb87..000000000 --- a/nuttx-configs/px4fmu-v2/common/ld.script +++ /dev/null @@ -1,150 +0,0 @@ -/**************************************************************************** - * configs/px4fmu/common/ld.script - * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and - * 256Kb of SRAM. SRAM is split up into three blocks: - * - * 1) 112Kb of SRAM beginning at address 0x2000:0000 - * 2) 16Kb of SRAM beginning at address 0x2001:c000 - * 3) 64Kb of SRAM beginning at address 0x2002:0000 - * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 - * - * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 - * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address range. - * - * The first 0x4000 of flash is reserved for the bootloader. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K - ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K -} - -OUTPUT_ARCH(arm) - -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - - /* - * This is a hack to make the newlib libm __errno() call - * use the NuttX get_errno_ptr() function. - */ - __errno = get_errno_ptr; - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 00257d546..e70320aaa 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -1,3 +1,179 @@ +############################################################################ +# configs/px4fmu-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4fmu-v2/common/Make.defs diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig.prev b/nuttx-configs/px4fmu-v2/nsh/defconfig.prev deleted file mode 100755 index 42910ce0a..000000000 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig.prev +++ /dev/null @@ -1,1067 +0,0 @@ -############################################################################ -# configs/px4fmu/nsh/defconfig -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip family. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization -# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH="arm" -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM4=y -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_BOARD="px4fmu-v2" -CONFIG_ARCH_BOARD_PX4FMU_V2=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_DRAM_SIZE=0x00040000 -CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_FPU=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=n -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=y -CONFIG_ARCH_MATH_H=y - -CONFIG_ARMV7M_CMNVECTOR=y -CONFIG_STM32_STM32F427=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=n -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=y - -# -# On-chip CCM SRAM configuration -# -# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need -# to do this if DMA is enabled to prevent non-DMA-able CCM memory from -# being a part of the stack. -# -CONFIG_STM32_CCMEXCLUDE=y - -# -# On-board FSMC SRAM configuration -# -# CONFIG_STM32_FSMC - Required. See below -# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) -# -# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the -# FSMC (as opposed to an LCD or FLASH). -# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space -# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space -# -#CONFIG_STM32_FSMC_SRAM=n -#CONFIG_HEAP2_BASE=0x64000000 -#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) - -# -# Individual subsystems can be enabled: -# -# This set is exhaustive for PX4FMU and should be safe to cut and -# paste into any other config. -# -# AHB1: -CONFIG_STM32_CRC=n -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_ETHMAC=n -CONFIG_STM32_OTGHS=n -# AHB2: -CONFIG_STM32_DCMI=n -CONFIG_STM32_CRYP=n -CONFIG_STM32_HASH=n -CONFIG_STM32_RNG=n -CONFIG_STM32_OTGFS=y -# AHB3: -CONFIG_STM32_FSMC=n -# APB1: -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=y -CONFIG_STM32_TIM4=y -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_TIM12=n -CONFIG_STM32_TIM13=n -CONFIG_STM32_TIM14=n -CONFIG_STM32_WWDG=y -CONFIG_STM32_IWDG=n -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI3=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART5=n -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=y -CONFIG_STM32_I2C3=n -CONFIG_STM32_CAN1=n -CONFIG_STM32_CAN2=n -CONFIG_STM32_DAC=n -CONFIG_STM32_PWR=y -# APB2: -CONFIG_STM32_TIM1=y -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -# Mostly owned by the px4io driver, but uploader needs this -CONFIG_STM32_USART6=y -# We use our own driver, but leave this on. -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -CONFIG_STM32_ADC3=n -CONFIG_STM32_SDIO=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SYSCFG=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y - -# -# Enable single wire support. If this is not defined, then this mode cannot -# be enabled. -# -CONFIG_STM32_USART_SINGLEWIRE=y - -# -# We want the flash prefetch on for max performance. -# -STM32_FLASH_PREFETCH=y - -# -# STM32F40xxx specific serial device driver settings -# -# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, -# tcflush, etc.). If this is not defined, then the terminal settings (baud, -# parity, etc.) are not configurable at runtime; serial streams cannot be -# flushed, etc. -# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port -# immediately after creating the /dev/console device. This is required -# if the console serial port has RX DMA enabled. -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_CONSOLE_REINIT=n -CONFIG_STANDARD_SERIAL=y - -CONFIG_UART8_SERIAL_CONSOLE=y - -#Mavlink messages can be bigger than 128 -CONFIG_USART1_TXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=256 -CONFIG_USART3_TXBUFSIZE=256 -CONFIG_UART4_TXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=128 -CONFIG_UART7_TXBUFSIZE=256 -CONFIG_UART8_TXBUFSIZE=256 - -CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART2_RXBUFSIZE=256 -CONFIG_USART3_RXBUFSIZE=256 -CONFIG_UART4_RXBUFSIZE=256 -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_UART7_RXBUFSIZE=256 -CONFIG_UART8_RXBUFSIZE=256 - -CONFIG_USART1_BAUD=115200 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 -CONFIG_UART4_BAUD=115200 -CONFIG_USART6_BAUD=115200 -CONFIG_UART7_BAUD=115200 -CONFIG_UART8_BAUD=57600 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 -CONFIG_UART4_BITS=8 -CONFIG_USART6_BITS=8 -CONFIG_UART7_BITS=8 -CONFIG_UART8_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 -CONFIG_UART4_PARITY=0 -CONFIG_USART6_PARITY=0 -CONFIG_UART7_PARITY=0 -CONFIG_UART8_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 -CONFIG_UART4_2STOP=0 -CONFIG_USART6_2STOP=0 -CONFIG_UART7_2STOP=0 -CONFIG_UART8_2STOP=0 - -CONFIG_USART1_RXDMA=y -CONFIG_USART2_RXDMA=y -CONFIG_USART3_RXDMA=n -CONFIG_UART4_RXDMA=n -CONFIG_USART6_RXDMA=y -CONFIG_UART7_RXDMA=n -CONFIG_UART8_RXDMA=n - -# -# STM32F40xxx specific SPI device driver settings -# -CONFIG_SPI_EXCHANGE=y -# DMA needs more work, not implemented on STM32F4x yet -#CONFIG_STM32_SPI_DMA=y - -# -# STM32F40xxx specific CAN device driver settings -# -# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or -# CONFIG_STM32_CAN2 must also be defined) -# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default -# Standard 11-bit IDs. -# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. -# Default: 8 -# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. -# Default: 4 -# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback -# mode for testing. The STM32 CAN driver does support loopback mode. -# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. -# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. -# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 -# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 -# -CONFIG_CAN=n -CONFIG_CAN_EXTID=n -#CONFIG_CAN_FIFOSIZE -#CONFIG_CAN_NPENDINGRTR -CONFIG_CAN_LOOPBACK=n -CONFIG_CAN1_BAUD=700000 -CONFIG_CAN2_BAUD=700000 - - -# XXX remove after integration testing -# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using -# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update -CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 -# Constant overhead for generating I2C start / stop conditions -CONFIG_STM32_I2CTIMEOUS_START_STOP=700 -# XXX this is bad and we want it gone -CONFIG_I2C_WRITEREAD=y - -# -# I2C configuration -# -CONFIG_I2C=y -CONFIG_I2C_POLLED=n -CONFIG_I2C_TRANSFER=y -CONFIG_I2C_TRACE=n -CONFIG_I2C_RESET=y -# XXX fixed per-transaction timeout -CONFIG_STM32_I2CTIMEOMS=10 - -# -# MTD support -# -CONFIG_MTD=y - -# XXX re-enable after integration testing - -# -# I2C configuration -# -#CONFIG_I2C=y -#CONFIG_I2C_POLLED=y -#CONFIG_I2C_TRANSFER=y -#CONFIG_I2C_TRACE=n -#CONFIG_I2C_RESET=y - -# Dynamic timeout -#CONFIG_STM32_I2C_DYNTIMEO=y -#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 -#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 - -# Fixed per-transaction timeout -#CONFIG_STM32_I2CTIMEOSEC=0 -#CONFIG_STM32_I2CTIMEOMS=10 - - - - - - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=y - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 192 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# -# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single -# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined -# then an additional, lower-priority work queue will also be created. This -# lower priority work queue is better suited for more extended processing -# (such as file system clean-up operations) -# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority -# worker thread. Default: 50 -# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread -# checks for work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower -# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SCHED_WAITPID - Enable the waitpid() API -# CONFIG_SCHED_ATEXIT - Enabled the atexit() API -# -CONFIG_USER_ENTRYPOINT="nsh_main" -#CONFIG_APPS_DIR= -CONFIG_DEBUG=y -CONFIG_DEBUG_VERBOSE=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEBUG_FS=n -CONFIG_DEBUG_GRAPHICS=n -CONFIG_DEBUG_LCD=n -CONFIG_DEBUG_USB=n -CONFIG_DEBUG_NET=n -CONFIG_DEBUG_RTC=n -CONFIG_DEBUG_ANALOG=n -CONFIG_DEBUG_PWM=n -CONFIG_DEBUG_CAN=n -CONFIG_DEBUG_I2C=n -CONFIG_DEBUG_INPUT=n - -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_MM_REGIONS=2 -CONFIG_ARCH_LOWPUTC=y -CONFIG_MSEC_PER_TICK=1 -CONFIG_RR_INTERVAL=0 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_START_YEAR=1970 -CONFIG_START_MONTH=1 -CONFIG_START_DAY=1 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=y -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=8 -CONFIG_FDCLONE_DISABLE=n -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SCHED_WORKQUEUE=y -CONFIG_SCHED_WORKPRIORITY=192 -CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 -CONFIG_SIG_SIGWORK=4 -CONFIG_SCHED_WAITPID=y -CONFIG_SCHED_ATEXIT=n - -# -# System Logging -# -# CONFIG_SYSLOG - Enables the System Logging feature. -# CONFIG_RAMLOG - Enables the RAM logging feature -# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. -# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all -# console output will be re-directed to a circular buffer in RAM. This -# is useful, for example, if the only console is a Telnet console. Then -# in that case, console output from non-Telnet threads will go to the -# circular buffer and can be viewed using the NSH 'dmesg' command. -# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging -# interface. If this feature is enabled (along with CONFIG_SYSLOG), -# then all debug output (only) will be re-directed to the circular -# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' -# command. -# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting -# for this driver on poll(). Default: 4 -# -# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the -# following may also be provided: -# -# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 -# - -CONFIG_SYSLOG=n -CONFIG_RAMLOG=n -CONFIG_RAMLOG_CONSOLE=n -CONFIG_RAMLOG_SYSLOG=n -#CONFIG_RAMLOG_NPOLLWAITERS -#CONFIG_RAMLOG_CONSOLE_BUFSIZE - -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=n -CONFIG_DISABLE_PTHREAD=n -CONFIG_DISABLE_SIGNALS=n -CONFIG_DISABLE_MQUEUE=n -CONFIG_DISABLE_MOUNTPOINT=n -CONFIG_DISABLE_ENVIRON=n -CONFIG_DISABLE_POLL=n - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") -# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: -# 5.1234567 -# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) -# -CONFIG_NOPRINTF_FIELDWIDTH=n -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_HAVE_LONG_LONG=y - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=y -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled -# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added -# to force automatic, line-oriented flushing the output buffer -# for putc(), fputc(), putchar(), puts(), fputs(), printf(), -# fprintf(), and vfprintf(). When a newline is encountered in -# the output string, the output buffer will be flushed. This -# (slightly) increases the NuttX footprint but supports the kind -# of behavior that people expect for printf(). -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=32 -CONFIG_MAX_TASK_ARGS=8 -CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 -CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=256 -CONFIG_STDIO_LINEBUFFER=y -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=4 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=50 -CONFIG_PREALLOC_TIMERS=50 - -# -# Filesystem configuration -# -# CONFIG_FS_FAT - Enable FAT filesystem support -# CONFIG_FAT_SECTORSIZE - Max supported sector size -# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 -# file name support. -# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims -# patents on FAT long file name technology. Please read the -# disclaimer in the top-level COPYING file and only enable this -# feature if you understand these issues. -# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the -# default, maximum long file name is 255 bytes. This can eat up -# a lot of memory (especially stack space). If you are willing -# to live with some non-standard, short long file names, then -# define this value. A good choice would be the same value as -# selected for CONFIG_NAME_MAX which will limit the visibility -# of longer file names anyway. -# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. -# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. -# This must have one of the values of 0xff or 0x00. -# Default: 0xff. -# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, -# don't both with file chunks smaller than this number of data bytes. -# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. -# Default: 255. -# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, -# don't both with file chunks smaller than this number of data bytes. -# Default: 32. -# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean -# packing files together toward the end of the file or, if file are -# deleted at the end of the file, clean up can simply mean erasing -# the end of FLASH memory so that it can be re-used again. However, -# doing this can also harm the life of the FLASH part because it can -# mean that the tail end of the FLASH is re-used too often. This -# threshold determines if/when it is worth erased the tail end of FLASH -# and making it available for re-use (and possible over-wear). -# Default: 8192. -# CONFIG_FS_ROMFS - Enable ROMFS filesystem support -# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this -# option will enable a limited form of memory mapping that is -# implemented by copying whole files into memory. -# -CONFIG_FS_FAT=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_MAXFNAME=32 -CONFIG_FS_NXFFS=y -CONFIG_NXFFS_MAXNAMLEN=32 -CONFIG_NXFFS_TAILTHRESHOLD=2048 -CONFIG_NXFFS_PREALLOCATED=y -CONFIG_FS_ROMFS=y -CONFIG_FS_BINFS=y - -# -# SPI-based MMC/SD driver -# -# CONFIG_MMCSD_NSLOTS -# Number of MMC/SD slots supported by the driver -# CONFIG_MMCSD_READONLY -# Provide read-only access (default is read/write) -# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. -# Default is 20MHz, current setting 24 MHz -# -#CONFIG_MMCSD=n -# XXX need to rejig this for SDIO -#CONFIG_MMCSD_SPI=y -#CONFIG_MMCSD_NSLOTS=1 -#CONFIG_MMCSD_READONLY=n -#CONFIG_MMCSD_SPICLOCK=24000000 - -# -# Maintain legacy build behavior (revisit) -# - -CONFIG_MMCSD=y -#CONFIG_MMCSD_SPI=y -CONFIG_MMCSD_SDIO=y -CONFIG_MTD=y - -# -# SPI-based MMC/SD driver -# -#CONFIG_MMCSD_NSLOTS=1 -#CONFIG_MMCSD_READONLY=n -#CONFIG_MMCSD_SPICLOCK=12500000 - -# -# STM32 SDIO-based MMC/SD driver -# -CONFIG_SDIO_DMA=y -#CONFIG_SDIO_PRI=128 -#CONFIG_SDIO_DMAPRIO -#CONFIG_SDIO_WIDTH_D1_ONLY -CONFIG_MMCSD_MULTIBLOCK_DISABLE=y -CONFIG_MMCSD_MMCSUPPORT=n -CONFIG_MMCSD_HAVECARDDETECT=n - -# -# Block driver buffering -# -# CONFIG_FS_READAHEAD -# Enable read-ahead buffering -# CONFIG_FS_WRITEBUFFER -# Enable write buffering -# -CONFIG_FS_READAHEAD=n -CONFIG_FS_WRITEBUFFER=n - -# -# RTC Configuration -# -# CONFIG_RTC - Enables general support for a hardware RTC. Specific -# architectures may require other specific settings. -# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple -# battery backed counter that keeps the time when power is down, and (2) -# A full date / time RTC the provides the date and time information, often -# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this -# second kind of RTC. In this case, the RTC is used to "seed" the normal -# NuttX timer and the NuttX system timer provides for higher resoution -# time. -# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, -# battery backed counter is used. There are two different implementations -# of such simple counters based on the time resolution of the counter: -# The typical RTC keeps time to resolution of 1 second, usually -# supporting a 32-bit time_t value. In this case, the RTC is used to -# "seed" the normal NuttX timer and the NuttX timer provides for higher -# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, -# then the RTC provides higher resolution time and completely replaces the -# system timer for purpose of date and time. -# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency -# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is -# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. -# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an -# alarm. A callback function will be executed when the alarm goes off -# -CONFIG_RTC=n -CONFIG_RTC_DATETIME=y -CONFIG_RTC_HIRES=n -CONFIG_RTC_FREQUENCY=n -CONFIG_RTC_ALARM=n - -# -# USB Device Configuration -# -# CONFIG_USBDEV -# Enables USB device support -# CONFIG_USBDEV_ISOCHRONOUS -# Build in extra support for isochronous endpoints -# CONFIG_USBDEV_DUALSPEED -# Hardware handles high and full speed operation (USB 2.0) -# CONFIG_USBDEV_SELFPOWERED -# Will cause USB features to indicate that the device is -# self-powered -# CONFIG_USBDEV_MAXPOWER -# Maximum power consumption in mA -# CONFIG_USBDEV_TRACE -# Enables USB tracing for debug -# CONFIG_USBDEV_TRACE_NRECORDS -# Number of trace entries to remember -# -CONFIG_USBDEV=y -CONFIG_USBDEV_ISOCHRONOUS=n -CONFIG_USBDEV_DUALSPEED=n -CONFIG_USBDEV_SELFPOWERED=y -CONFIG_USBDEV_REMOTEWAKEUP=n -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USBDEV_TRACE=n -CONFIG_USBDEV_TRACE_NRECORDS=512 - -# -# USB serial device class driver (Standard CDC ACM class) -# -# CONFIG_CDCACM -# Enable compilation of the USB serial driver -# CONFIG_CDCACM_CONSOLE -# Configures the CDC/ACM serial port as the console device. -# CONFIG_CDCACM_EP0MAXPACKET -# Endpoint 0 max packet size. Default 64 -# CONFIG_CDCACM_EPINTIN -# The logical 7-bit address of a hardware endpoint that supports -# interrupt IN operation. Default 2. -# CONFIG_CDCACM_EPINTIN_FSSIZE -# Max package size for the interrupt IN endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPINTIN_HSSIZE -# Max package size for the interrupt IN endpoint if high speed mode. -# Default 64 -# CONFIG_CDCACM_EPBULKOUT -# The logical 7-bit address of a hardware endpoint that supports -# bulk OUT operation. Default 4. -# CONFIG_CDCACM_EPBULKOUT_FSSIZE -# Max package size for the bulk OUT endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPBULKOUT_HSSIZE -# Max package size for the bulk OUT endpoint if high speed mode. -# Default 512. -# CONFIG_CDCACM_EPBULKIN -# The logical 7-bit address of a hardware endpoint that supports -# bulk IN operation. Default 3. -# CONFIG_CDCACM_EPBULKIN_FSSIZE -# Max package size for the bulk IN endpoint if full speed mode. -# Default 64. -# CONFIG_CDCACM_EPBULKIN_HSSIZE -# Max package size for the bulk IN endpoint if high speed mode. -# Default 512. -# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS -# The number of write/read requests that can be in flight. -# Default 256. -# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR -# The vendor ID code/string. Default 0x0525 and "NuttX" -# 0x0525 is the Netchip vendor and should not be used in any -# products. This default VID was selected for compatibility with -# the Linux CDC ACM default VID. -# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR -# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" -# 0xa4a7 was selected for compatibility with the Linux CDC ACM -# default PID. -# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE -# Size of the serial receive/transmit buffers. Default 256. -# -CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n -#CONFIG_CDCACM_EP0MAXPACKET -CONFIG_CDCACM_EPINTIN=1 -#CONFIG_CDCACM_EPINTIN_FSSIZE -#CONFIG_CDCACM_EPINTIN_HSSIZE -CONFIG_CDCACM_EPBULKOUT=3 -#CONFIG_CDCACM_EPBULKOUT_FSSIZE -#CONFIG_CDCACM_EPBULKOUT_HSSIZE -CONFIG_CDCACM_EPBULKIN=2 -#CONFIG_CDCACM_EPBULKIN_FSSIZE -#CONFIG_CDCACM_EPBULKIN_HSSIZE -#CONFIG_CDCACM_NWRREQS -#CONFIG_CDCACM_NRDREQS -CONFIG_CDCACM_VENDORID=0x26AC -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTID=0x0010 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" -#CONFIG_CDCACM_RXBUFSIZE -#CONFIG_CDCACM_TXBUFSIZE - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# -# If CONFIG_NSH_TELNET is selected: -# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size -# CONFIG_NSH_DHCPC - Obtain address using DHCP -# CONFIG_NSH_IPADDR - Provides static IP address -# CONFIG_NSH_DRIPADDR - Provides static router IP address -# CONFIG_NSH_NETMASK - Provides static network mask -# CONFIG_NSH_NOMAC - Use a bogus MAC address -# -# If CONFIG_NSH_ROMFSETC is selected: -# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint -# CONFIG_NSH_INITSCRIPT - Relative path to init script -# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor -# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size -# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor -# CONFIG_NSH_FATSECTSIZE - FAT FS sector size -# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors -# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint -# -CONFIG_BUILTIN=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_FILEIOSIZE=512 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAX_ARGUMENTS=12 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_DISABLESCRIPT=n -CONFIG_NSH_DISABLEBG=n -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_CONSOLE=y -CONFIG_NSH_USBCONSOLE=n -#CONFIG_NSH_USBCONDEV="/dev/ttyACM0" -CONFIG_NSH_TELNET=n -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_IOBUFFER_SIZE=512 -CONFIG_NSH_DHCPC=n -CONFIG_NSH_NOMAC=y -CONFIG_NSH_IPADDR=0x0a000002 -CONFIG_NSH_DRIPADDR=0x0a000001 -CONFIG_NSH_NETMASK=0xffffff00 -CONFIG_NSH_ROMFSMOUNTPT="/etc" -CONFIG_NSH_INITSCRIPT="init.d/rcS" -CONFIG_NSH_ROMFSDEVNO=0 -CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard -CONFIG_NSH_FATDEVNO=1 -CONFIG_NSH_FATSECTSIZE=512 -CONFIG_NSH_FATNSECTORS=1024 -CONFIG_NSH_FATMOUNTPT=/tmp - -# -# Architecture-specific NSH options -# -#CONFIG_NSH_MMCSDSPIPORTNO=3 -CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDMINOR=0 - - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3240G-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -# Idle thread needs 4096 bytes -# default 1 KB is not enough -# 4096 bytes -CONFIG_IDLETHREAD_STACKSIZE=6000 -# USERMAIN stack size probably needs to be around 4096 bytes -CONFIG_USERMAIN_STACKSIZE=4096 -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_PTHREAD_STACK_DEFAULT=2048 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= - -# enable bindir -CONFIG_APPS_BINDIR=y diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script new file mode 100644 index 000000000..1be39fb87 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v1/Kconfig b/nuttx-configs/px4io-v1/Kconfig deleted file mode 100644 index fbf74d7f0..000000000 --- a/nuttx-configs/px4io-v1/Kconfig +++ /dev/null @@ -1,21 +0,0 @@ -# -# For a description of the syntax of this configuration file, -# see misc/tools/kconfig-language.txt. -# - -if ARCH_BOARD_PX4IO_V1 - -config HRT_TIMER - bool "High resolution timer support" - default y - ---help--- - Enable high resolution timer for PPM capture and system clocks. - -config HRT_PPM - bool "PPM input capture" - default y - depends on HRT_TIMER - ---help--- - Enable PPM input capture via HRT (for CPPM / PPM sum RC inputs) - -endif diff --git a/nuttx-configs/px4io-v1/README.txt b/nuttx-configs/px4io-v1/README.txt deleted file mode 100755 index 9b1615f42..000000000 --- a/nuttx-configs/px4io-v1/README.txt +++ /dev/null @@ -1,806 +0,0 @@ -README -====== - -This README discusses issues unique to NuttX configurations for the -STMicro STM3210E-EVAL development board. - -Contents -======== - - - Development Environment - - GNU Toolchain Options - - IDEs - - NuttX buildroot Toolchain - - DFU and JTAG - - OpenOCD - - LEDs - - Temperature Sensor - - RTC - - STM3210E-EVAL-specific Configuration Options - - Configurations - -Development Environment -======================= - - Either Linux or Cygwin on Windows can be used for the development environment. - The source has been built only using the GNU toolchain (see below). Other - toolchains will likely cause problems. Testing was performed using the Cygwin - environment because the Raisonance R-Link emulatator and some RIDE7 development tools - were used and those tools works only under Windows. - -GNU Toolchain Options -===================== - - The NuttX make system has been modified to support the following different - toolchain options. - - 1. The CodeSourcery GNU toolchain, - 2. The devkitARM GNU toolchain, - 3. Raisonance GNU toolchain, or - 4. The NuttX buildroot Toolchain (see below). - - All testing has been conducted using the NuttX buildroot toolchain. However, - the make system is setup to default to use the devkitARM toolchain. To use - the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to - add one of the following configuration options to your .config (or defconfig) - file: - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux - CONFIG_STM32_DEVKITARM=y : devkitARM under Windows - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) - - If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify - the PATH in the setenv.h file if your make cannot find the tools. - - NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are - Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot - toolchains are Cygwin and/or Linux native toolchains. There are several limitations - to using a Windows based toolchain in a Cygwin environment. The three biggest are: - - 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are - performed automatically in the Cygwin makefiles using the 'cygpath' utility - but you might easily find some new path problems. If so, check out 'cygpath -w' - - 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links - are used in Nuttx (e.g., include/arch). The make system works around these - problems for the Windows tools by copying directories instead of linking them. - But this can also cause some confusion for you: For example, you may edit - a file in a "linked" directory and find that your changes had no effect. - That is because you are building the copy of the file in the "fake" symbolic - directory. If you use a Windows toolchain, you should get in the habit of - making like this: - - make clean_context all - - An alias in your .bashrc file might make that less painful. - - 3. Dependencies are not made when using Windows versions of the GCC. This is - because the dependencies are generated using Windows pathes which do not - work with the Cygwin make. - - Support has been added for making dependencies with the windows-native toolchains. - That support can be enabled by modifying your Make.defs file as follows: - - - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" - - If you have problems with the dependency build (for example, if you are not - building on C:), then you may need to modify tools/mkdeps.sh - - NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization - level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with - -Os. - - NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that - the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM - path or will get the wrong version of make. - -IDEs -==== - - NuttX is built using command-line make. It can be used with an IDE, but some - effort will be required to create the project (There is a simple RIDE project - in the RIDE subdirectory). - - Makefile Build - -------------- - Under Eclipse, it is pretty easy to set up an "empty makefile project" and - simply use the NuttX makefile to build the system. That is almost for free - under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty - makefile project in order to work with Windows (Google for "Eclipse Cygwin" - - there is a lot of help on the internet). - - Native Build - ------------ - Here are a few tips before you start that effort: - - 1) Select the toolchain that you will be using in your .config file - 2) Start the NuttX build at least one time from the Cygwin command line - before trying to create your project. This is necessary to create - certain auto-generated files and directories that will be needed. - 3) Set up include pathes: You will need include/, arch/arm/src/stm32, - arch/arm/src/common, arch/arm/src/armv7-m, and sched/. - 4) All assembly files need to have the definition option -D __ASSEMBLY__ - on the command line. - - Startup files will probably cause you some headaches. The NuttX startup file - is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX - one time from the Cygwin command line in order to obtain the pre-built - startup object needed by RIDE. - -NuttX buildroot Toolchain -========================= - - A GNU GCC-based toolchain is assumed. The files */setenv.sh should - be modified to point to the correct path to the Cortex-M3 GCC toolchain (if - different from the default in your PATH variable). - - If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX - SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). - This GNU toolchain builds and executes in the Linux or Cygwin environment. - - 1. You must have already configured Nuttx in /nuttx. - - cd tools - ./configure.sh stm3210e-eval/ - - 2. Download the latest buildroot package into - - 3. unpack the buildroot tarball. The resulting directory may - have versioning information on it like buildroot-x.y.z. If so, - rename /buildroot-x.y.z to /buildroot. - - 4. cd /buildroot - - 5. cp configs/cortexm3-defconfig-4.3.3 .config - - 6. make oldconfig - - 7. make - - 8. Edit setenv.h, if necessary, so that the PATH variable includes - the path to the newly built binaries. - - See the file configs/README.txt in the buildroot source tree. That has more - detailed PLUS some special instructions that you will need to follow if you are - building a Cortex-M3 toolchain for Cygwin under Windows. - -DFU and JTAG -============ - - Enbling Support for the DFU Bootloader - -------------------------------------- - The linker files in these projects can be configured to indicate that you - will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) - loader or via some JTAG emulator. You can specify the DFU bootloader by - adding the following line: - - CONFIG_STM32_DFU=y - - to your .config file. Most of the configurations in this directory are set - up to use the DFU loader. - - If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning - of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed - to make space for the DFU loader and 0x08003000 is where the DFU loader expects - to find new applications at boot time. If you need to change that origin for some - other bootloader, you will need to edit the file(s) ld.script.dfu for each - configuration. - - The DFU SE PC-based software is available from the STMicro website, - http://www.st.com. General usage instructions: - - 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU - file (nuttx.dfu)... see below for details. - 2. Connect the STM3210E-EVAL board to your computer using a USB - cable. - 3. Start the DFU loader on the STM3210E-EVAL board. You do this by - resetting the board while holding the "Key" button. Windows should - recognize that the DFU loader has been installed. - 3. Run the DFU SE program to load nuttx.dfu into FLASH. - - What if the DFU loader is not in FLASH? The loader code is available - inside of the Demo dirctory of the USBLib ZIP file that can be downloaded - from the STMicro Website. You can build it using RIDE (or other toolchains); - you will need a JTAG emulator to burn it into FLASH the first time. - - In order to use STMicro's built-in DFU loader, you will have to get - the NuttX binary into a special format with a .dfu extension. The - DFU SE PC_based software installation includes a file "DFU File Manager" - conversion program that a file in Intel Hex format to the special DFU - format. When you successfully build NuttX, you will find a file called - nutt.ihx in the top-level directory. That is the file that you should - provide to the DFU File Manager. You will need to rename it to nuttx.hex - in order to find it with the DFU File Manager. You will end up with - a file called nuttx.dfu that you can use with the STMicro DFU SE program. - - Enabling JTAG - ------------- - If you are not using the DFU, then you will probably also need to enable - JTAG support. By default, all JTAG support is disabled but there NuttX - configuration options to enable JTAG in various different ways. - - These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO - MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the - Cortex debug port. The default state in this port is for all JTAG support - to be disable. - - CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full - SWJ (JTAG-DP + SW-DP) - - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable - full SWJ (JTAG-DP + SW-DP) but without JNTRST. - - CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP - disabled and SW-DP enabled - - The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 - which disable JTAG-DP and SW-DP. - -OpenOCD -======= - -I have also used OpenOCD with the STM3210E-EVAL. In this case, I used -the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh -for more information. Using the script: - -1) Start the OpenOCD GDB server - - cd - configs/stm3210e-eval/tools/oocd.sh $PWD - -2) Load Nuttx - - cd - arm-none-eabi-gdb nuttx - gdb> target remote localhost:3333 - gdb> mon reset - gdb> mon halt - gdb> load nuttx - -3) Running NuttX - - gdb> mon reset - gdb> c - -LEDs -==== - -The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the -board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is -defined. In that case, the usage by the board port is defined in -include/board.h and src/up_leds.c. The LEDs are used to encode OS-related -events as follows: - - SYMBOL Meaning LED1* LED2 LED3 LED4 - ---------------- ----------------------- ----- ----- ----- ----- - LED_STARTED NuttX has been started ON OFF OFF OFF - LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF - LED_IRQSENABLED Interrupts enabled ON ON OFF OFF - LED_STACKCREATED Idle stack created OFF OFF ON OFF - LED_INIRQ In an interrupt** ON N/C N/C OFF - LED_SIGNAL In a signal handler*** N/C ON N/C OFF - LED_ASSERTION An assertion failed ON ON N/C OFF - LED_PANIC The system has crashed N/C N/C N/C ON - LED_IDLE STM32 is is sleep mode (Optional, not used) - - * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot - and these LEDs will give you some indication of where the failure was - ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow - is because of timer interupts that result in the LED being illuminated - on a small proportion of the time. -*** LED2 may also flicker normally if signals are processed. - -Temperature Sensor -================== - -Support for the on-board LM-75 temperature sensor is available. This supported -has been verified, but has not been included in any of the available the -configurations. To set up the temperature sensor, add the following to the -NuttX configuration file - - CONFIG_I2C=y - CONFIG_I2C_LM75=y - -Then you can implement logic like the following to use the temperature sensor: - - #include - #include - - ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ - fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ - ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ - bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ - -More complex temperature sensor operations are also available. See the IOCTAL -commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions -of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the -arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). - -RTC -=== - - The STM32 RTC may configured using the following settings. - - CONFIG_RTC - Enables general support for a hardware RTC. Specific - architectures may require other specific settings. - CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 - second, usually supporting a 32-bit time_t value. In this case, - the RTC is used to "seed" the normal NuttX timer and the - NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES - is enabled in the NuttX configuration, then the RTC provides higher - resolution time and completely replaces the system timer for purpose of - date and time. - CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the - frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES - is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. - CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. - A callback function will be executed when the alarm goes off - - In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts - are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. - A BKP register is incremented on each overflow interrupt creating, effectively, - a 48-bit RTC counter. - - In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled - (because the next overflow is not expected until the year 2106. - - WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The - overflow interrupt may be lost even if the STM32 is powered down only momentarily. - Therefore hi-res solution is only useful in systems where the power is always on. - -STM3210E-EVAL-specific Configuration Options -============================================ - - CONFIG_ARCH - Identifies the arch/ subdirectory. This should - be set to: - - CONFIG_ARCH=arm - - CONFIG_ARCH_family - For use in C code: - - CONFIG_ARCH_ARM=y - - CONFIG_ARCH_architecture - For use in C code: - - CONFIG_ARCH_CORTEXM3=y - - CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory - - CONFIG_ARCH_CHIP=stm32 - - CONFIG_ARCH_CHIP_name - For use in C code to identify the exact - chip: - - CONFIG_ARCH_CHIP_STM32F103ZET6 - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock - configuration features. - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n - - CONFIG_ARCH_BOARD - Identifies the configs subdirectory and - hence, the board that supports the particular chip or SoC. - - CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) - - CONFIG_ARCH_BOARD_name - For use in C code - - CONFIG_ARCH_BOARD_STM3210E_EVAL=y - - CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation - of delay loops - - CONFIG_ENDIAN_BIG - define if big endian (default is little - endian) - - CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): - - CONFIG_DRAM_SIZE=0x00010000 (64Kb) - - CONFIG_DRAM_START - The start address of installed DRAM - - CONFIG_DRAM_START=0x20000000 - - CONFIG_DRAM_END - Last address+1 of installed RAM - - CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) - - CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization - - CONFIG_ARCH_IRQPRIO=y - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that - have LEDs - - CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt - stack. If defined, this symbol is the size of the interrupt - stack in bytes. If not defined, the user task stacks will be - used during interrupt handling. - - CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. - - CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that - cause a 100 second delay during boot-up. This 100 second delay - serves no purpose other than it allows you to calibratre - CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure - the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until - the delay actually is 100 seconds. - - Individual subsystems can be enabled: - AHB - --- - CONFIG_STM32_DMA1 - CONFIG_STM32_DMA2 - CONFIG_STM32_CRC - CONFIG_STM32_FSMC - CONFIG_STM32_SDIO - - APB1 - ---- - CONFIG_STM32_TIM2 - CONFIG_STM32_TIM3 - CONFIG_STM32_TIM4 - CONFIG_STM32_TIM5 - CONFIG_STM32_TIM6 - CONFIG_STM32_TIM7 - CONFIG_STM32_WWDG - CONFIG_STM32_SPI2 - CONFIG_STM32_SPI4 - CONFIG_STM32_USART2 - CONFIG_STM32_USART3 - CONFIG_STM32_UART4 - CONFIG_STM32_UART5 - CONFIG_STM32_I2C1 - CONFIG_STM32_I2C2 - CONFIG_STM32_USB - CONFIG_STM32_CAN - CONFIG_STM32_BKP - CONFIG_STM32_PWR - CONFIG_STM32_DAC1 - CONFIG_STM32_DAC2 - CONFIG_STM32_USB - - APB2 - ---- - CONFIG_STM32_ADC1 - CONFIG_STM32_ADC2 - CONFIG_STM32_TIM1 - CONFIG_STM32_SPI1 - CONFIG_STM32_TIM8 - CONFIG_STM32_USART1 - CONFIG_STM32_ADC3 - - Timer and I2C devices may need to the following to force power to be applied - unconditionally at power up. (Otherwise, the device is powered when it is - initialized). - - CONFIG_STM32_FORCEPOWER - - Timer devices may be used for different purposes. One special purpose is - to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn - is defined (as above) then the following may also be defined to indicate that - the timer is intended to be used for pulsed output modulation, ADC conversion, - or DAC conversion. Note that ADC/DAC require two definition: Not only do you have - to assign the timer (n) for used by the ADC or DAC, but then you also have to - configure which ADC or DAC (m) it is assigned to. - - CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 - CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 - CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 - CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 - CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 - - For each timer that is enabled for PWM usage, we need the following additional - configuration settings: - - CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} - - NOTE: The STM32 timers are each capable of generating different signals on - each of the four channels with different duty cycles. That capability is - not supported by this driver: Only one output channel per timer. - - Alternate pin mappings (should not be used with the STM3210E-EVAL board): - - CONFIG_STM32_TIM1_FULL_REMAP - CONFIG_STM32_TIM1_PARTIAL_REMAP - CONFIG_STM32_TIM2_FULL_REMAP - CONFIG_STM32_TIM2_PARTIAL_REMAP_1 - CONFIG_STM32_TIM2_PARTIAL_REMAP_2 - CONFIG_STM32_TIM3_FULL_REMAP - CONFIG_STM32_TIM3_PARTIAL_REMAP - CONFIG_STM32_TIM4_REMAP - CONFIG_STM32_USART1_REMAP - CONFIG_STM32_USART2_REMAP - CONFIG_STM32_USART3_FULL_REMAP - CONFIG_STM32_USART3_PARTIAL_REMAP - CONFIG_STM32_SPI1_REMAP - CONFIG_STM32_SPI3_REMAP - CONFIG_STM32_I2C1_REMAP - CONFIG_STM32_CAN1_FULL_REMAP - CONFIG_STM32_CAN1_PARTIAL_REMAP - CONFIG_STM32_CAN2_REMAP - - JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): - CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - but without JNTRST. - CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled - - STM32F103Z specific device driver settings - - CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART - m (m=4,5) for the console and ttys0 (default is the USART1). - CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. - This specific the size of the receive buffer - CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before - being sent. This specific the size of the transmit buffer - CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be - CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. - CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity - CONFIG_U[S]ARTn_2STOP - Two stop bits - - CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI - support. Non-interrupt-driven, poll-waiting is recommended if the - interrupt rate would be to high in the interrupt driven case. - CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. - Cannot be used with CONFIG_STM32_SPI_INTERRUPT. - - CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO - and CONFIG_STM32_DMA2. - CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 - CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. - Default: Medium - CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: - 4-bit transfer mode. - - STM3210E-EVAL CAN Configuration - - CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or - CONFIG_STM32_CAN2 must also be defined) - CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default - Standard 11-bit IDs. - CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. - Default: 8 - CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. - Default: 4 - CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback - mode for testing. The STM32 CAN driver does support loopback mode. - CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. - CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. - CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 - CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 - CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an - dump of all CAN registers. - - STM3210E-EVAL LCD Hardware Configuration - - CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" - support. Default is this 320x240 "landscape" orientation - (this setting is informative only... not used). - CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" - orientation support. In this orientation, the STM3210E-EVAL's - LCD ribbon cable is at the bottom of the display. Default is - 320x240 "landscape" orientation. - CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse - portrait" orientation support. In this orientation, the - STM3210E-EVAL's LCD ribbon cable is at the top of the display. - Default is 320x240 "landscape" orientation. - CONFIG_LCD_BACKLIGHT - Define to support a backlight. - CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an - adjustable backlight will be provided using timer 1 to generate - various pulse widthes. The granularity of the settings is - determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or - CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight - is provided. - CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears - to be a shift in the returned data. This value fixes the offset. - Default 5. - - The LCD driver dynamically selects the LCD based on the reported LCD - ID value. However, code size can be reduced by suppressing support for - individual LCDs using: - - CONFIG_STM32_AM240320_DISABLE - CONFIG_STM32_SPFD5408B_DISABLE - CONFIG_STM32_R61580_DISABLE - -Configurations -============== - -Each STM3210E-EVAL configuration is maintained in a sudirectory and -can be selected as follow: - - cd tools - ./configure.sh stm3210e-eval/ - cd - - . ./setenv.sh - -Where is one of the following: - - buttons: - -------- - - Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and - button interrupts. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - - composite - --------- - - This configuration exercises a composite USB interface consisting - of a CDC/ACM device and a USB mass storage device. This configuration - uses apps/examples/composite. - - nsh and nsh2: - ------------ - Configure the NuttShell (nsh) located at examples/nsh. - - Differences between the two NSH configurations: - - =========== ======================= ================================ - nsh nsh2 - =========== ======================= ================================ - Toolchain: NuttX buildroot for Codesourcery for Windows (1) - Linux or Cygwin (1,2) - ----------- ----------------------- -------------------------------- - Loader: DfuSe DfuSe - ----------- ----------------------- -------------------------------- - Serial Debug output: USART1 Debug output: USART1 - Console: NSH output: USART1 NSH output: USART1 (3) - ----------- ----------------------- -------------------------------- - microSD Yes Yes - Support - ----------- ----------------------- -------------------------------- - FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y - Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) - ----------- ----------------------- -------------------------------- - Support for No Yes - Built-in - Apps - ----------- ----------------------- -------------------------------- - Built-in None apps/examples/nx - Apps apps/examples/nxhello - apps/examples/usbstorage (5) - =========== ======================= ================================ - - (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh - to set up the correct PATH variable for whichever toolchain you - may use. - (2) Since DfuSe is assumed, this configuration may only work under - Cygwin without modification. - (3) When any other device other than /dev/console is used for a user - interface, (1) linefeeds (\n) will not be expanded to carriage return - / linefeeds \r\n). You will need to configure your terminal program - to account for this. And (2) input is not automatically echoed so - you will have to turn local echo on. - (4) Microsoft holds several patents related to the design of - long file names in the FAT file system. Please refer to the - details in the top-level COPYING file. Please do not use FAT - long file name unless you are familiar with these patent issues. - (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), - Caution should be used to assure that the SD drive is not in use when - the USB storage device is configured. Specifically, the SD driver - should be unmounted like: - - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH - ... - nsh> umount /mnd/sdcard # Unmount before connecting USB!!! - nsh> msconn # Connect the USB storage device - ... - nsh> msdis # Disconnect USB storate device - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount - - Failure to do this could result in corruption of the SD card format. - - nx: - --- - An example using the NuttX graphics system (NX). This example - focuses on general window controls, movement, mouse and keyboard - input. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxlines: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing lines on the background in various - orientations. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxtext: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing text on the background while pop-up - windows occur. Text should continue to update normally with - or without the popup windows present. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - NOTE: When I tried building this example with the CodeSourcery - tools, I got a hardfault inside of its libgcc. I haven't - retested since then, but beware if you choose to change the - toolchain. - - ostest: - ------ - This configuration directory, performs a simple OS test using - examples/ostest. By default, this project assumes that you are - using the DFU bootloader. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - RIDE - ---- - This configuration builds a trivial bring-up binary. It is - useful only because it words with the RIDE7 IDE and R-Link debugger. - - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - - usbserial: - --------- - This configuration directory exercises the USB serial class - driver at examples/usbserial. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - USB debug output can be enabled as by changing the following - settings in the configuration file: - - -CONFIG_DEBUG=n - -CONFIG_DEBUG_VERBOSE=n - -CONFIG_DEBUG_USB=n - +CONFIG_DEBUG=y - +CONFIG_DEBUG_VERBOSE=y - +CONFIG_DEBUG_USB=y - - -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n - -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n - +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y - +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y - - By default, the usbserial example uses the Prolific PL2303 - serial/USB converter emulation. The example can be modified - to use the CDC/ACM serial class by making the following changes - to the configuration file: - - -CONFIG_PL2303=y - +CONFIG_PL2303=n - - -CONFIG_CDCACM=n - +CONFIG_CDCACM=y - - The example can also be converted to use the alternative - USB serial example at apps/examples/usbterm by changing the - following: - - -CONFIGURED_APPS += examples/usbserial - +CONFIGURED_APPS += examples/usbterm - - In either the original appconfig file (before configuring) - or in the final apps/.config file (after configuring). - - usbstorage: - ---------- - This configuration directory exercises the USB mass storage - class driver at examples/usbstorage. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - diff --git a/nuttx-configs/px4io-v1/common/Make.defs b/nuttx-configs/px4io-v1/common/Make.defs deleted file mode 100644 index 74b183067..000000000 --- a/nuttx-configs/px4io-v1/common/Make.defs +++ /dev/null @@ -1,171 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m3 \ - -mthumb \ - -march=armv7-m - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4io-v1/common/ld.script b/nuttx-configs/px4io-v1/common/ld.script deleted file mode 100755 index 69c2f9cb2..000000000 --- a/nuttx-configs/px4io-v1/common/ld.script +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * configs/stm3210e-eval/nsh/ld.script - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and - * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, - * FLASH memory is aliased to address 0x0000:0000 where the code expects to - * begin execution by jumping to the entry point in the 0x0800:0000 address - * range. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K -} - -OUTPUT_ARCH(arm) -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.glue_7) - *(.glue_7t) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4io-v1/common/setenv.sh b/nuttx-configs/px4io-v1/common/setenv.sh deleted file mode 100755 index ff9a4bf8a..000000000 --- a/nuttx-configs/px4io-v1/common/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/include/drv_i2c_device.h b/nuttx-configs/px4io-v1/include/drv_i2c_device.h deleted file mode 100644 index 02582bc09..000000000 --- a/nuttx-configs/px4io-v1/include/drv_i2c_device.h +++ /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. - * - ****************************************************************************/ - - /** - * @file A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -extern void i2c_fsm_init(uint8_t *buffer, size_t buffer_size); -extern bool i2c_fsm(void); diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index c7f6effd9..712631f47 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -1,3 +1,166 @@ +############################################################################ +# configs/px4io-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4io-v1/common/Make.defs diff --git a/nuttx-configs/px4io-v1/scripts/ld.script b/nuttx-configs/px4io-v1/scripts/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx-configs/px4io-v1/scripts/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v1/src/README.txt b/nuttx-configs/px4io-v1/src/README.txt deleted file mode 100644 index d4eda82fd..000000000 --- a/nuttx-configs/px4io-v1/src/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v1/src/drv_i2c_device.c b/nuttx-configs/px4io-v1/src/drv_i2c_device.c deleted file mode 100644 index 1f5931ae5..000000000 --- a/nuttx-configs/px4io-v1/src/drv_i2c_device.c +++ /dev/null @@ -1,618 +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 A simple, polled I2C slave-mode driver. - * - * The master writes to and reads from a byte buffer, which the caller - * can update inbetween calls to the FSM. - */ - -#include - -#include "stm32_i2c.h" - -#include - -/* - * I2C register definitions. - */ -#define I2C_BASE STM32_I2C1_BASE - -#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) - -#define rCR1 REG(STM32_I2C_CR1_OFFSET) -#define rCR2 REG(STM32_I2C_CR2_OFFSET) -#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) -#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) -#define rDR REG(STM32_I2C_DR_OFFSET) -#define rSR1 REG(STM32_I2C_SR1_OFFSET) -#define rSR2 REG(STM32_I2C_SR2_OFFSET) -#define rCCR REG(STM32_I2C_CCR_OFFSET) -#define rTRISE REG(STM32_I2C_TRISE_OFFSET) - -/* - * "event" values (cr2 << 16 | cr1) as described in the ST DriverLib - */ -#define I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED ((uint32_t)0x00020002) /* BUSY and ADDR flags */ -#define I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED ((uint32_t)0x00060082) /* TRA, BUSY, TXE and ADDR flags */ -#define I2C_EVENT_SLAVE_BYTE_RECEIVED ((uint32_t)0x00020040) /* BUSY and RXNE flags */ -#define I2C_EVENT_SLAVE_STOP_DETECTED ((uint32_t)0x00000010) /* STOPF flag */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTED ((uint32_t)0x00060084) /* TRA, BUSY, TXE and BTF flags */ -#define I2C_EVENT_SLAVE_BYTE_TRANSMITTING ((uint32_t)0x00060080) /* TRA, BUSY and TXE flags */ -#define I2C_EVENT_SLAVE_ACK_FAILURE ((uint32_t)0x00000400) /* AF flag */ - -/** - * States implemented by the I2C FSM. - */ -enum fsm_state { - BAD_PHASE, // must be zero, default exit on a bad state transition - - WAIT_FOR_MASTER, - - /* write from master */ - WAIT_FOR_COMMAND, - RECEIVE_COMMAND, - RECEIVE_DATA, - HANDLE_COMMAND, - - /* read from master */ - WAIT_TO_SEND, - SEND_STATUS, - SEND_DATA, - - NUM_STATES -}; - -/** - * Events recognised by the I2C FSM. - */ -enum fsm_event { - /* automatic transition */ - AUTO, - - /* write from master */ - ADDRESSED_WRITE, - BYTE_RECEIVED, - STOP_RECEIVED, - - /* read from master */ - ADDRESSED_READ, - BYTE_SENDABLE, - ACK_FAILED, - - NUM_EVENTS -}; - -/** - * Context for the I2C FSM - */ -static struct fsm_context { - enum fsm_state state; - - /* XXX want to eliminate these */ - uint8_t command; - uint8_t status; - - uint8_t *data_ptr; - uint32_t data_count; - - size_t buffer_size; - uint8_t *buffer; -} context; - -/** - * Structure defining one FSM state and its outgoing transitions. - */ -struct fsm_transition { - void (*handler)(void); - enum fsm_state next_state[NUM_EVENTS]; -}; - -static bool i2c_command_received; - -static void fsm_event(enum fsm_event event); - -static void go_bad(void); -static void go_wait_master(void); - -static void go_wait_command(void); -static void go_receive_command(void); -static void go_receive_data(void); -static void go_handle_command(void); - -static void go_wait_send(void); -static void go_send_status(void); -static void go_send_buffer(void); - -/** - * The FSM state graph. - */ -static const struct fsm_transition fsm[NUM_STATES] = { - [BAD_PHASE] = { - .handler = go_bad, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - [WAIT_FOR_MASTER] = { - .handler = go_wait_master, - .next_state = { - [ADDRESSED_WRITE] = WAIT_FOR_COMMAND, - [ADDRESSED_READ] = WAIT_TO_SEND, - }, - }, - - /* write from master*/ - [WAIT_FOR_COMMAND] = { - .handler = go_wait_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_COMMAND, - [STOP_RECEIVED] = WAIT_FOR_MASTER, - }, - }, - [RECEIVE_COMMAND] = { - .handler = go_receive_command, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [RECEIVE_DATA] = { - .handler = go_receive_data, - .next_state = { - [BYTE_RECEIVED] = RECEIVE_DATA, - [STOP_RECEIVED] = HANDLE_COMMAND, - }, - }, - [HANDLE_COMMAND] = { - .handler = go_handle_command, - .next_state = { - [AUTO] = WAIT_FOR_MASTER, - }, - }, - - /* buffer send */ - [WAIT_TO_SEND] = { - .handler = go_wait_send, - .next_state = { - [BYTE_SENDABLE] = SEND_STATUS, - }, - }, - [SEND_STATUS] = { - .handler = go_send_status, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, - [SEND_DATA] = { - .handler = go_send_buffer, - .next_state = { - [BYTE_SENDABLE] = SEND_DATA, - [ACK_FAILED] = WAIT_FOR_MASTER, - }, - }, -}; - - -/* debug support */ -#if 1 -struct fsm_logentry { - char kind; - uint32_t code; -}; - -#define LOG_ENTRIES 32 -static struct fsm_logentry fsm_log[LOG_ENTRIES]; -int fsm_logptr; -#define LOG_NEXT(_x) (((_x) + 1) % LOG_ENTRIES) -#define LOGx(_kind, _code) \ - do { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr = LOG_NEXT(fsm_logptr); \ - fsm_log[fsm_logptr].kind = 0; \ - } while(0) - -#define LOG(_kind, _code) \ - do {\ - if (fsm_logptr < LOG_ENTRIES) { \ - fsm_log[fsm_logptr].kind = _kind; \ - fsm_log[fsm_logptr].code = _code; \ - fsm_logptr++;\ - }\ - }while(0) - -#else -#define LOG(_kind, _code) -#endif - - -static void i2c_setclock(uint32_t frequency); - -/** - * Initialise I2C - * - */ -void -i2c_fsm_init(uint8_t *buffer, size_t buffer_size) -{ - /* save the buffer */ - context.buffer = buffer; - context.buffer_size = buffer_size; - - // initialise the FSM - context.status = 0; - context.command = 0; - context.state = BAD_PHASE; - fsm_event(AUTO); - -#if 0 - // enable the i2c block clock and reset it - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); - - // configure the i2c GPIOs - stm32_configgpio(GPIO_I2C1_SCL); - stm32_configgpio(GPIO_I2C1_SDA); - - // set the peripheral clock to match the APB clock - rCR2 = STM32_PCLK1_FREQUENCY / 1000000; - - // configure for 100kHz operation - i2c_setclock(100000); - - // enable i2c - rCR1 = I2C_CR1_PE; -#endif -} - -/** - * Run the I2C FSM for some period. - * - * @return True if the buffer has been updated by a command. - */ -bool -i2c_fsm(void) -{ - uint32_t event; - int idle_iterations = 0; - - for (;;) { - // handle bus error states by discarding the current operation - if (rSR1 & I2C_SR1_BERR) { - context.state = WAIT_FOR_MASTER; - rSR1 = ~I2C_SR1_BERR; - } - - // we do not anticipate over/underrun errors as clock-stretching is enabled - - // fetch the most recent event - event = ((rSR2 << 16) | rSR1) & 0x00ffffff; - - // generate FSM events based on I2C events - switch (event) { - case I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED: - LOG('w', 0); - fsm_event(ADDRESSED_WRITE); - break; - - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: - LOG('r', 0); - fsm_event(ADDRESSED_READ); - break; - - case I2C_EVENT_SLAVE_BYTE_RECEIVED: - LOG('R', 0); - fsm_event(BYTE_RECEIVED); - break; - - case I2C_EVENT_SLAVE_STOP_DETECTED: - LOG('s', 0); - fsm_event(STOP_RECEIVED); - break; - - case I2C_EVENT_SLAVE_BYTE_TRANSMITTING: - //case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: - LOG('T', 0); - fsm_event(BYTE_SENDABLE); - break; - - case I2C_EVENT_SLAVE_ACK_FAILURE: - LOG('a', 0); - fsm_event(ACK_FAILED); - break; - - default: - idle_iterations++; -// if ((event) && (event != 0x00020000)) -// LOG('e', event); - break; - } - - /* if we have just received something, drop out and let the caller handle it */ - if (i2c_command_received) { - i2c_command_received = false; - return true; - } - - /* if we have done nothing recently, drop out and let the caller have a slice */ - if (idle_iterations > 1000) - return false; - } -} - -/** - * Update the FSM with an event - * - * @param event New event. - */ -static void -fsm_event(enum fsm_event event) -{ - // move to the next state - context.state = fsm[context.state].next_state[event]; - - LOG('f', context.state); - - // call the state entry handler - if (fsm[context.state].handler) { - fsm[context.state].handler(); - } -} - -static void -go_bad() -{ - LOG('B', 0); - fsm_event(AUTO); -} - -/** - * Wait for the master to address us. - * - */ -static void -go_wait_master() -{ - // We currently don't have a command byte. - // - context.command = '\0'; - - // The data pointer starts pointing to the start of the data buffer. - // - context.data_ptr = context.buffer; - - // The data count is either: - // - the size of the data buffer - // - some value less than or equal the size of the data buffer during a write or a read - // - context.data_count = context.buffer_size; - - // (re)enable the peripheral, clear the stop event flag in - // case we just finished receiving data - rCR1 |= I2C_CR1_PE; - - // clear the ACK failed flag in case we just finished sending data - rSR1 = ~I2C_SR1_AF; -} - -/** - * Prepare to receive a command byte. - * - */ -static void -go_wait_command() -{ - // NOP -} - -/** - * Command byte has been received, save it and prepare to handle the data. - * - */ -static void -go_receive_command() -{ - - // fetch the command byte - context.command = (uint8_t)rDR; - LOG('c', context.command); - -} - -/** - * Receive a data byte. - * - */ -static void -go_receive_data() -{ - uint8_t d; - - // fetch the byte - d = (uint8_t)rDR; - LOG('d', d); - - // if we have somewhere to put it, do so - if (context.data_count) { - *context.data_ptr++ = d; - context.data_count--; - } -} - -/** - * Handle a command once the host is done sending it to us. - * - */ -static void -go_handle_command() -{ - // presume we are happy with the command - context.status = 0; - - // make a note that the buffer contains a fresh command - i2c_command_received = true; - - // kick along to the next state - fsm_event(AUTO); -} - -/** - * Wait to be able to send the status byte. - * - */ -static void -go_wait_send() -{ - // NOP -} - -/** - * Send the status byte. - * - */ -static void -go_send_status() -{ - rDR = context.status; - LOG('?', context.status); -} - -/** - * Send a data or pad byte. - * - */ -static void -go_send_buffer() -{ - if (context.data_count) { - LOG('D', *context.data_ptr); - rDR = *(context.data_ptr++); - context.data_count--; - } else { - LOG('-', 0); - rDR = 0xff; - } -} - -/* cribbed directly from the NuttX master driver */ -static void -i2c_setclock(uint32_t frequency) -{ - uint16_t cr1; - uint16_t ccr; - uint16_t trise; - uint16_t freqmhz; - uint16_t speed; - - /* Disable the selected I2C peripheral to configure TRISE */ - - cr1 = rCR1; - rCR1 &= ~I2C_CR1_PE; - - /* Update timing and control registers */ - - freqmhz = (uint16_t)(STM32_PCLK1_FREQUENCY / 1000000); - ccr = 0; - - /* Configure speed in standard mode */ - - if (frequency <= 100000) { - /* Standard mode speed calculation */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency << 1)); - - /* The CCR fault must be >= 4 */ - - if (speed < 4) { - /* Set the minimum allowed value */ - - speed = 4; - } - ccr |= speed; - - /* Set Maximum Rise Time for standard mode */ - - trise = freqmhz + 1; - - /* Configure speed in fast mode */ - } else { /* (frequency <= 400000) */ - /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ - -#ifdef CONFIG_I2C_DUTY16_9 - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); - - /* Set DUTY and fast speed bits */ - - ccr |= (I2C_CCR_DUTY|I2C_CCR_FS); -#else - /* Fast mode speed calculation with Tlow/Thigh = 2 */ - - speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 3)); - - /* Set fast speed bit */ - - ccr |= I2C_CCR_FS; -#endif - - /* Verify that the CCR speed value is nonzero */ - - if (speed < 1) { - /* Set the minimum allowed value */ - - speed = 1; - } - ccr |= speed; - - /* Set Maximum Rise Time for fast mode */ - - trise = (uint16_t)(((freqmhz * 300) / 1000) + 1); - } - - /* Write the new values of the CCR and TRISE registers */ - - rCCR = ccr; - rTRISE = trise; - - /* Bit 14 of OAR1 must be configured and kept at 1 */ - - rOAR1 = I2C_OAR1_ONE); - - /* Re-enable the peripheral (or not) */ - - rCR1 = cr1; -} diff --git a/nuttx-configs/px4io-v2/README.txt b/nuttx-configs/px4io-v2/README.txt deleted file mode 100755 index 9b1615f42..000000000 --- a/nuttx-configs/px4io-v2/README.txt +++ /dev/null @@ -1,806 +0,0 @@ -README -====== - -This README discusses issues unique to NuttX configurations for the -STMicro STM3210E-EVAL development board. - -Contents -======== - - - Development Environment - - GNU Toolchain Options - - IDEs - - NuttX buildroot Toolchain - - DFU and JTAG - - OpenOCD - - LEDs - - Temperature Sensor - - RTC - - STM3210E-EVAL-specific Configuration Options - - Configurations - -Development Environment -======================= - - Either Linux or Cygwin on Windows can be used for the development environment. - The source has been built only using the GNU toolchain (see below). Other - toolchains will likely cause problems. Testing was performed using the Cygwin - environment because the Raisonance R-Link emulatator and some RIDE7 development tools - were used and those tools works only under Windows. - -GNU Toolchain Options -===================== - - The NuttX make system has been modified to support the following different - toolchain options. - - 1. The CodeSourcery GNU toolchain, - 2. The devkitARM GNU toolchain, - 3. Raisonance GNU toolchain, or - 4. The NuttX buildroot Toolchain (see below). - - All testing has been conducted using the NuttX buildroot toolchain. However, - the make system is setup to default to use the devkitARM toolchain. To use - the CodeSourcery, devkitARM or Raisonance GNU toolchain, you simply need to - add one of the following configuration options to your .config (or defconfig) - file: - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux - CONFIG_STM32_DEVKITARM=y : devkitARM under Windows - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) - - If you are not using CONFIG_STM32_BUILDROOT, then you may also have to modify - the PATH in the setenv.h file if your make cannot find the tools. - - NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are - Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot - toolchains are Cygwin and/or Linux native toolchains. There are several limitations - to using a Windows based toolchain in a Cygwin environment. The three biggest are: - - 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are - performed automatically in the Cygwin makefiles using the 'cygpath' utility - but you might easily find some new path problems. If so, check out 'cygpath -w' - - 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links - are used in Nuttx (e.g., include/arch). The make system works around these - problems for the Windows tools by copying directories instead of linking them. - But this can also cause some confusion for you: For example, you may edit - a file in a "linked" directory and find that your changes had no effect. - That is because you are building the copy of the file in the "fake" symbolic - directory. If you use a Windows toolchain, you should get in the habit of - making like this: - - make clean_context all - - An alias in your .bashrc file might make that less painful. - - 3. Dependencies are not made when using Windows versions of the GCC. This is - because the dependencies are generated using Windows pathes which do not - work with the Cygwin make. - - Support has been added for making dependencies with the windows-native toolchains. - That support can be enabled by modifying your Make.defs file as follows: - - - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" - - If you have problems with the dependency build (for example, if you are not - building on C:), then you may need to modify tools/mkdeps.sh - - NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization - level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with - -Os. - - NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that - the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM - path or will get the wrong version of make. - -IDEs -==== - - NuttX is built using command-line make. It can be used with an IDE, but some - effort will be required to create the project (There is a simple RIDE project - in the RIDE subdirectory). - - Makefile Build - -------------- - Under Eclipse, it is pretty easy to set up an "empty makefile project" and - simply use the NuttX makefile to build the system. That is almost for free - under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty - makefile project in order to work with Windows (Google for "Eclipse Cygwin" - - there is a lot of help on the internet). - - Native Build - ------------ - Here are a few tips before you start that effort: - - 1) Select the toolchain that you will be using in your .config file - 2) Start the NuttX build at least one time from the Cygwin command line - before trying to create your project. This is necessary to create - certain auto-generated files and directories that will be needed. - 3) Set up include pathes: You will need include/, arch/arm/src/stm32, - arch/arm/src/common, arch/arm/src/armv7-m, and sched/. - 4) All assembly files need to have the definition option -D __ASSEMBLY__ - on the command line. - - Startup files will probably cause you some headaches. The NuttX startup file - is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX - one time from the Cygwin command line in order to obtain the pre-built - startup object needed by RIDE. - -NuttX buildroot Toolchain -========================= - - A GNU GCC-based toolchain is assumed. The files */setenv.sh should - be modified to point to the correct path to the Cortex-M3 GCC toolchain (if - different from the default in your PATH variable). - - If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX - SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). - This GNU toolchain builds and executes in the Linux or Cygwin environment. - - 1. You must have already configured Nuttx in /nuttx. - - cd tools - ./configure.sh stm3210e-eval/ - - 2. Download the latest buildroot package into - - 3. unpack the buildroot tarball. The resulting directory may - have versioning information on it like buildroot-x.y.z. If so, - rename /buildroot-x.y.z to /buildroot. - - 4. cd /buildroot - - 5. cp configs/cortexm3-defconfig-4.3.3 .config - - 6. make oldconfig - - 7. make - - 8. Edit setenv.h, if necessary, so that the PATH variable includes - the path to the newly built binaries. - - See the file configs/README.txt in the buildroot source tree. That has more - detailed PLUS some special instructions that you will need to follow if you are - building a Cortex-M3 toolchain for Cygwin under Windows. - -DFU and JTAG -============ - - Enbling Support for the DFU Bootloader - -------------------------------------- - The linker files in these projects can be configured to indicate that you - will be loading code using STMicro built-in USB Device Firmware Upgrade (DFU) - loader or via some JTAG emulator. You can specify the DFU bootloader by - adding the following line: - - CONFIG_STM32_DFU=y - - to your .config file. Most of the configurations in this directory are set - up to use the DFU loader. - - If CONFIG_STM32_DFU is defined, the code will not be positioned at the beginning - of FLASH (0x08000000) but will be offset to 0x08003000. This offset is needed - to make space for the DFU loader and 0x08003000 is where the DFU loader expects - to find new applications at boot time. If you need to change that origin for some - other bootloader, you will need to edit the file(s) ld.script.dfu for each - configuration. - - The DFU SE PC-based software is available from the STMicro website, - http://www.st.com. General usage instructions: - - 1. Convert the NuttX Intel Hex file (nuttx.ihx) into a special DFU - file (nuttx.dfu)... see below for details. - 2. Connect the STM3210E-EVAL board to your computer using a USB - cable. - 3. Start the DFU loader on the STM3210E-EVAL board. You do this by - resetting the board while holding the "Key" button. Windows should - recognize that the DFU loader has been installed. - 3. Run the DFU SE program to load nuttx.dfu into FLASH. - - What if the DFU loader is not in FLASH? The loader code is available - inside of the Demo dirctory of the USBLib ZIP file that can be downloaded - from the STMicro Website. You can build it using RIDE (or other toolchains); - you will need a JTAG emulator to burn it into FLASH the first time. - - In order to use STMicro's built-in DFU loader, you will have to get - the NuttX binary into a special format with a .dfu extension. The - DFU SE PC_based software installation includes a file "DFU File Manager" - conversion program that a file in Intel Hex format to the special DFU - format. When you successfully build NuttX, you will find a file called - nutt.ihx in the top-level directory. That is the file that you should - provide to the DFU File Manager. You will need to rename it to nuttx.hex - in order to find it with the DFU File Manager. You will end up with - a file called nuttx.dfu that you can use with the STMicro DFU SE program. - - Enabling JTAG - ------------- - If you are not using the DFU, then you will probably also need to enable - JTAG support. By default, all JTAG support is disabled but there NuttX - configuration options to enable JTAG in various different ways. - - These configurations effect the setting of the SWJ_CFG[2:0] bits in the AFIO - MAPR register. These bits are used to configure the SWJ and trace alternate function I/Os. The SWJ (SerialWire JTAG) supports JTAG or SWD access to the - Cortex debug port. The default state in this port is for all JTAG support - to be disable. - - CONFIG_STM32_JTAG_FULL_ENABLE - sets SWJ_CFG[2:0] to 000 which enables full - SWJ (JTAG-DP + SW-DP) - - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - sets SWJ_CFG[2:0] to 001 which enable - full SWJ (JTAG-DP + SW-DP) but without JNTRST. - - CONFIG_STM32_JTAG_SW_ENABLE - sets SWJ_CFG[2:0] to 010 which would set JTAG-DP - disabled and SW-DP enabled - - The default setting (none of the above defined) is SWJ_CFG[2:0] set to 100 - which disable JTAG-DP and SW-DP. - -OpenOCD -======= - -I have also used OpenOCD with the STM3210E-EVAL. In this case, I used -the Olimex USB ARM OCD. See the script in configs/stm3210e-eval/tools/oocd.sh -for more information. Using the script: - -1) Start the OpenOCD GDB server - - cd - configs/stm3210e-eval/tools/oocd.sh $PWD - -2) Load Nuttx - - cd - arm-none-eabi-gdb nuttx - gdb> target remote localhost:3333 - gdb> mon reset - gdb> mon halt - gdb> load nuttx - -3) Running NuttX - - gdb> mon reset - gdb> c - -LEDs -==== - -The STM3210E-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the -board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is -defined. In that case, the usage by the board port is defined in -include/board.h and src/up_leds.c. The LEDs are used to encode OS-related -events as follows: - - SYMBOL Meaning LED1* LED2 LED3 LED4 - ---------------- ----------------------- ----- ----- ----- ----- - LED_STARTED NuttX has been started ON OFF OFF OFF - LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF - LED_IRQSENABLED Interrupts enabled ON ON OFF OFF - LED_STACKCREATED Idle stack created OFF OFF ON OFF - LED_INIRQ In an interrupt** ON N/C N/C OFF - LED_SIGNAL In a signal handler*** N/C ON N/C OFF - LED_ASSERTION An assertion failed ON ON N/C OFF - LED_PANIC The system has crashed N/C N/C N/C ON - LED_IDLE STM32 is is sleep mode (Optional, not used) - - * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot - and these LEDs will give you some indication of where the failure was - ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow - is because of timer interupts that result in the LED being illuminated - on a small proportion of the time. -*** LED2 may also flicker normally if signals are processed. - -Temperature Sensor -================== - -Support for the on-board LM-75 temperature sensor is available. This supported -has been verified, but has not been included in any of the available the -configurations. To set up the temperature sensor, add the following to the -NuttX configuration file - - CONFIG_I2C=y - CONFIG_I2C_LM75=y - -Then you can implement logic like the following to use the temperature sensor: - - #include - #include - - ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ - fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ - ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ - bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ - -More complex temperature sensor operations are also available. See the IOCTAL -commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions -of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the -arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). - -RTC -=== - - The STM32 RTC may configured using the following settings. - - CONFIG_RTC - Enables general support for a hardware RTC. Specific - architectures may require other specific settings. - CONFIG_RTC_HIRES - The typical RTC keeps time to resolution of 1 - second, usually supporting a 32-bit time_t value. In this case, - the RTC is used to "seed" the normal NuttX timer and the - NuttX timer provides for higher resoution time. If CONFIG_RTC_HIRES - is enabled in the NuttX configuration, then the RTC provides higher - resolution time and completely replaces the system timer for purpose of - date and time. - CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the - frequency of the high resolution RTC must be provided. If CONFIG_RTC_HIRES - is not defined, CONFIG_RTC_FREQUENCY is assumed to be one. - CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an alarm. - A callback function will be executed when the alarm goes off - - In hi-res mode, the STM32 RTC operates only at 16384Hz. Overflow interrupts - are handled when the 32-bit RTC counter overflows every 3 days and 43 minutes. - A BKP register is incremented on each overflow interrupt creating, effectively, - a 48-bit RTC counter. - - In the lo-res mode, the RTC operates at 1Hz. Overflow interrupts are not handled - (because the next overflow is not expected until the year 2106. - - WARNING: Overflow interrupts are lost whenever the STM32 is powered down. The - overflow interrupt may be lost even if the STM32 is powered down only momentarily. - Therefore hi-res solution is only useful in systems where the power is always on. - -STM3210E-EVAL-specific Configuration Options -============================================ - - CONFIG_ARCH - Identifies the arch/ subdirectory. This should - be set to: - - CONFIG_ARCH=arm - - CONFIG_ARCH_family - For use in C code: - - CONFIG_ARCH_ARM=y - - CONFIG_ARCH_architecture - For use in C code: - - CONFIG_ARCH_CORTEXM3=y - - CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory - - CONFIG_ARCH_CHIP=stm32 - - CONFIG_ARCH_CHIP_name - For use in C code to identify the exact - chip: - - CONFIG_ARCH_CHIP_STM32F103ZET6 - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock - configuration features. - - CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n - - CONFIG_ARCH_BOARD - Identifies the configs subdirectory and - hence, the board that supports the particular chip or SoC. - - CONFIG_ARCH_BOARD=stm3210e_eval (for the STM3210E-EVAL development board) - - CONFIG_ARCH_BOARD_name - For use in C code - - CONFIG_ARCH_BOARD_STM3210E_EVAL=y - - CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation - of delay loops - - CONFIG_ENDIAN_BIG - define if big endian (default is little - endian) - - CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): - - CONFIG_DRAM_SIZE=0x00010000 (64Kb) - - CONFIG_DRAM_START - The start address of installed DRAM - - CONFIG_DRAM_START=0x20000000 - - CONFIG_DRAM_END - Last address+1 of installed RAM - - CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) - - CONFIG_ARCH_IRQPRIO - The STM32F103Z supports interrupt prioritization - - CONFIG_ARCH_IRQPRIO=y - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that - have LEDs - - CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt - stack. If defined, this symbol is the size of the interrupt - stack in bytes. If not defined, the user task stacks will be - used during interrupt handling. - - CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions - - CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. - - CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that - cause a 100 second delay during boot-up. This 100 second delay - serves no purpose other than it allows you to calibratre - CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure - the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until - the delay actually is 100 seconds. - - Individual subsystems can be enabled: - AHB - --- - CONFIG_STM32_DMA1 - CONFIG_STM32_DMA2 - CONFIG_STM32_CRC - CONFIG_STM32_FSMC - CONFIG_STM32_SDIO - - APB1 - ---- - CONFIG_STM32_TIM2 - CONFIG_STM32_TIM3 - CONFIG_STM32_TIM4 - CONFIG_STM32_TIM5 - CONFIG_STM32_TIM6 - CONFIG_STM32_TIM7 - CONFIG_STM32_WWDG - CONFIG_STM32_SPI2 - CONFIG_STM32_SPI4 - CONFIG_STM32_USART2 - CONFIG_STM32_USART3 - CONFIG_STM32_UART4 - CONFIG_STM32_UART5 - CONFIG_STM32_I2C1 - CONFIG_STM32_I2C2 - CONFIG_STM32_USB - CONFIG_STM32_CAN - CONFIG_STM32_BKP - CONFIG_STM32_PWR - CONFIG_STM32_DAC1 - CONFIG_STM32_DAC2 - CONFIG_STM32_USB - - APB2 - ---- - CONFIG_STM32_ADC1 - CONFIG_STM32_ADC2 - CONFIG_STM32_TIM1 - CONFIG_STM32_SPI1 - CONFIG_STM32_TIM8 - CONFIG_STM32_USART1 - CONFIG_STM32_ADC3 - - Timer and I2C devices may need to the following to force power to be applied - unconditionally at power up. (Otherwise, the device is powered when it is - initialized). - - CONFIG_STM32_FORCEPOWER - - Timer devices may be used for different purposes. One special purpose is - to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn - is defined (as above) then the following may also be defined to indicate that - the timer is intended to be used for pulsed output modulation, ADC conversion, - or DAC conversion. Note that ADC/DAC require two definition: Not only do you have - to assign the timer (n) for used by the ADC or DAC, but then you also have to - configure which ADC or DAC (m) it is assigned to. - - CONFIG_STM32_TIMn_PWM Reserve timer n for use by PWM, n=1,..,8 - CONFIG_STM32_TIMn_ADC Reserve timer n for use by ADC, n=1,..,8 - CONFIG_STM32_TIMn_ADCm Reserve timer n to trigger ADCm, n=1,..,8, m=1,..,3 - CONFIG_STM32_TIMn_DAC Reserve timer n for use by DAC, n=1,..,8 - CONFIG_STM32_TIMn_DACm Reserve timer n to trigger DACm, n=1,..,8, m=1,..,2 - - For each timer that is enabled for PWM usage, we need the following additional - configuration settings: - - CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} - - NOTE: The STM32 timers are each capable of generating different signals on - each of the four channels with different duty cycles. That capability is - not supported by this driver: Only one output channel per timer. - - Alternate pin mappings (should not be used with the STM3210E-EVAL board): - - CONFIG_STM32_TIM1_FULL_REMAP - CONFIG_STM32_TIM1_PARTIAL_REMAP - CONFIG_STM32_TIM2_FULL_REMAP - CONFIG_STM32_TIM2_PARTIAL_REMAP_1 - CONFIG_STM32_TIM2_PARTIAL_REMAP_2 - CONFIG_STM32_TIM3_FULL_REMAP - CONFIG_STM32_TIM3_PARTIAL_REMAP - CONFIG_STM32_TIM4_REMAP - CONFIG_STM32_USART1_REMAP - CONFIG_STM32_USART2_REMAP - CONFIG_STM32_USART3_FULL_REMAP - CONFIG_STM32_USART3_PARTIAL_REMAP - CONFIG_STM32_SPI1_REMAP - CONFIG_STM32_SPI3_REMAP - CONFIG_STM32_I2C1_REMAP - CONFIG_STM32_CAN1_FULL_REMAP - CONFIG_STM32_CAN1_PARTIAL_REMAP - CONFIG_STM32_CAN2_REMAP - - JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): - CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) - but without JNTRST. - CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled - - STM32F103Z specific device driver settings - - CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART - m (m=4,5) for the console and ttys0 (default is the USART1). - CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. - This specific the size of the receive buffer - CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before - being sent. This specific the size of the transmit buffer - CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be - CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. - CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity - CONFIG_U[S]ARTn_2STOP - Two stop bits - - CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI - support. Non-interrupt-driven, poll-waiting is recommended if the - interrupt rate would be to high in the interrupt driven case. - CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. - Cannot be used with CONFIG_STM32_SPI_INTERRUPT. - - CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO - and CONFIG_STM32_DMA2. - CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 - CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. - Default: Medium - CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: - 4-bit transfer mode. - - STM3210E-EVAL CAN Configuration - - CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or - CONFIG_STM32_CAN2 must also be defined) - CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default - Standard 11-bit IDs. - CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. - Default: 8 - CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. - Default: 4 - CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback - mode for testing. The STM32 CAN driver does support loopback mode. - CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. - CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. - CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 - CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 - CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an - dump of all CAN registers. - - STM3210E-EVAL LCD Hardware Configuration - - CONFIG_LCD_LANDSCAPE - Define for 320x240 display "landscape" - support. Default is this 320x240 "landscape" orientation - (this setting is informative only... not used). - CONFIG_LCD_PORTRAIT - Define for 240x320 display "portrait" - orientation support. In this orientation, the STM3210E-EVAL's - LCD ribbon cable is at the bottom of the display. Default is - 320x240 "landscape" orientation. - CONFIG_LCD_RPORTRAIT - Define for 240x320 display "reverse - portrait" orientation support. In this orientation, the - STM3210E-EVAL's LCD ribbon cable is at the top of the display. - Default is 320x240 "landscape" orientation. - CONFIG_LCD_BACKLIGHT - Define to support a backlight. - CONFIG_LCD_PWM - If CONFIG_STM32_TIM1 is also defined, then an - adjustable backlight will be provided using timer 1 to generate - various pulse widthes. The granularity of the settings is - determined by CONFIG_LCD_MAXPOWER. If CONFIG_LCD_PWM (or - CONFIG_STM32_TIM1) is not defined, then a simple on/off backlight - is provided. - CONFIG_LCD_RDSHIFT - When reading 16-bit gram data, there appears - to be a shift in the returned data. This value fixes the offset. - Default 5. - - The LCD driver dynamically selects the LCD based on the reported LCD - ID value. However, code size can be reduced by suppressing support for - individual LCDs using: - - CONFIG_STM32_AM240320_DISABLE - CONFIG_STM32_SPFD5408B_DISABLE - CONFIG_STM32_R61580_DISABLE - -Configurations -============== - -Each STM3210E-EVAL configuration is maintained in a sudirectory and -can be selected as follow: - - cd tools - ./configure.sh stm3210e-eval/ - cd - - . ./setenv.sh - -Where is one of the following: - - buttons: - -------- - - Uses apps/examples/buttons to exercise STM3210E-EVAL buttons and - button interrupts. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - - composite - --------- - - This configuration exercises a composite USB interface consisting - of a CDC/ACM device and a USB mass storage device. This configuration - uses apps/examples/composite. - - nsh and nsh2: - ------------ - Configure the NuttShell (nsh) located at examples/nsh. - - Differences between the two NSH configurations: - - =========== ======================= ================================ - nsh nsh2 - =========== ======================= ================================ - Toolchain: NuttX buildroot for Codesourcery for Windows (1) - Linux or Cygwin (1,2) - ----------- ----------------------- -------------------------------- - Loader: DfuSe DfuSe - ----------- ----------------------- -------------------------------- - Serial Debug output: USART1 Debug output: USART1 - Console: NSH output: USART1 NSH output: USART1 (3) - ----------- ----------------------- -------------------------------- - microSD Yes Yes - Support - ----------- ----------------------- -------------------------------- - FAT FS CONFIG_FAT_LCNAME=y CONFIG_FAT_LCNAME=y - Config CONFIG_FAT_LFN=n CONFIG_FAT_LFN=y (4) - ----------- ----------------------- -------------------------------- - Support for No Yes - Built-in - Apps - ----------- ----------------------- -------------------------------- - Built-in None apps/examples/nx - Apps apps/examples/nxhello - apps/examples/usbstorage (5) - =========== ======================= ================================ - - (1) You will probably need to modify nsh/setenv.sh or nsh2/setenv.sh - to set up the correct PATH variable for whichever toolchain you - may use. - (2) Since DfuSe is assumed, this configuration may only work under - Cygwin without modification. - (3) When any other device other than /dev/console is used for a user - interface, (1) linefeeds (\n) will not be expanded to carriage return - / linefeeds \r\n). You will need to configure your terminal program - to account for this. And (2) input is not automatically echoed so - you will have to turn local echo on. - (4) Microsoft holds several patents related to the design of - long file names in the FAT file system. Please refer to the - details in the top-level COPYING file. Please do not use FAT - long file name unless you are familiar with these patent issues. - (5) When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), - Caution should be used to assure that the SD drive is not in use when - the USB storage device is configured. Specifically, the SD driver - should be unmounted like: - - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH - ... - nsh> umount /mnd/sdcard # Unmount before connecting USB!!! - nsh> msconn # Connect the USB storage device - ... - nsh> msdis # Disconnect USB storate device - nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount - - Failure to do this could result in corruption of the SD card format. - - nx: - --- - An example using the NuttX graphics system (NX). This example - focuses on general window controls, movement, mouse and keyboard - input. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxlines: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing lines on the background in various - orientations. - - CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - nxtext: - ------ - Another example using the NuttX graphics system (NX). This - example focuses on placing text on the background while pop-up - windows occur. Text should continue to update normally with - or without the popup windows present. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - CONFIG_LCD_RPORTRAIT=y : 240x320 reverse portrait - - NOTE: When I tried building this example with the CodeSourcery - tools, I got a hardfault inside of its libgcc. I haven't - retested since then, but beware if you choose to change the - toolchain. - - ostest: - ------ - This configuration directory, performs a simple OS test using - examples/ostest. By default, this project assumes that you are - using the DFU bootloader. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - RIDE - ---- - This configuration builds a trivial bring-up binary. It is - useful only because it words with the RIDE7 IDE and R-Link debugger. - - CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows - - usbserial: - --------- - This configuration directory exercises the USB serial class - driver at examples/usbserial. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - - USB debug output can be enabled as by changing the following - settings in the configuration file: - - -CONFIG_DEBUG=n - -CONFIG_DEBUG_VERBOSE=n - -CONFIG_DEBUG_USB=n - +CONFIG_DEBUG=y - +CONFIG_DEBUG_VERBOSE=y - +CONFIG_DEBUG_USB=y - - -CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=n - -CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=n - -CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=n - +CONFIG_EXAMPLES_USBSERIAL_TRACEINIT=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECLASS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS=y - +CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER=y - +CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS=y - - By default, the usbserial example uses the Prolific PL2303 - serial/USB converter emulation. The example can be modified - to use the CDC/ACM serial class by making the following changes - to the configuration file: - - -CONFIG_PL2303=y - +CONFIG_PL2303=n - - -CONFIG_CDCACM=n - +CONFIG_CDCACM=y - - The example can also be converted to use the alternative - USB serial example at apps/examples/usbterm by changing the - following: - - -CONFIGURED_APPS += examples/usbserial - +CONFIGURED_APPS += examples/usbterm - - In either the original appconfig file (before configuring) - or in the final apps/.config file (after configuring). - - usbstorage: - ---------- - This configuration directory exercises the USB mass storage - class driver at examples/usbstorage. See examples/README.txt for - more information. - - CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin - diff --git a/nuttx-configs/px4io-v2/common/Make.defs b/nuttx-configs/px4io-v2/common/Make.defs deleted file mode 100644 index 7f782b5b2..000000000 --- a/nuttx-configs/px4io-v2/common/Make.defs +++ /dev/null @@ -1,175 +0,0 @@ -############################################################################ -# configs/px4fmu/common/Make.defs -# -# Copyright (C) 2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# -# Generic Make.defs for the PX4FMU -# Do not specify/use this file directly - it is included by config-specific -# Make.defs in the per-config directories. -# - -include ${TOPDIR}/tools/Config.mk - -# -# We only support building with the ARM bare-metal toolchain from -# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. -# -CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI - -include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs - -CC = $(CROSSDEV)gcc -CXX = $(CROSSDEV)g++ -CPP = $(CROSSDEV)gcc -E -LD = $(CROSSDEV)ld -AR = $(CROSSDEV)ar rcs -NM = $(CROSSDEV)nm -OBJCOPY = $(CROSSDEV)objcopy -OBJDUMP = $(CROSSDEV)objdump - -MAXOPTIMIZATION = -O3 -ARCHCPUFLAGS = -mcpu=cortex-m3 \ - -mthumb \ - -march=armv7-m - -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - -# use our linker script -LDSCRIPT = ld.script - -ifeq ($(WINTOOL),y) - # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" - ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" - ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" -else - ifeq ($(PX4_WINTOOL),y) - # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/copydir.sh - DIRUNLINK = $(TOPDIR)/tools/unlink.sh - MKDEP = $(TOPDIR)/tools/mknulldeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - else - # Linux/Cygwin-native toolchain - MKDEP = $(TOPDIR)/tools/mkdeps.sh - ARCHINCLUDES = -I. -isystem $(TOPDIR)/include - ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx - ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) - endif -endif - -# tool versions -ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} -ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} - -# optimisation flags -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -fno-strict-aliasing \ - -fno-strength-reduce \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections - -ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") -ARCHOPTIMIZATION += -g -endif - -ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -ARCHWARNINGS = -Wall \ - -Wextra \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter -# -Wcast-qual - generates spurious noreturn attribute warnings, try again later -# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code -# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives - -ARCHCWARNINGS = $(ARCHWARNINGS) \ - -Wbad-function-cast \ - -Wstrict-prototypes \ - -Wold-style-declaration \ - -Wmissing-parameter-type \ - -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants -ARCHWARNINGSXX = $(ARCHWARNINGS) -ARCHDEFINES = -ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 - -# this seems to be the only way to add linker flags -EXTRA_LIBS += --warn-common \ - --gc-sections - -CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common -CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) -CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) -CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -AFLAGS = $(CFLAGS) -D__ASSEMBLY__ - -NXFLATLDFLAGS1 = -r -d -warn-common -NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections -LDNXFLATFLAGS = -e main -s 2048 - -OBJEXT = .o -LIBEXT = .a -EXEEXT = - -# produce partially-linked $1 from files in $2 -define PRELINK - @echo "PRELINK: $1" - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 -endef - -HOSTCC = gcc -HOSTINCLUDES = -I. -HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe -HOSTLDFLAGS = - diff --git a/nuttx-configs/px4io-v2/common/ld.script b/nuttx-configs/px4io-v2/common/ld.script deleted file mode 100755 index 69c2f9cb2..000000000 --- a/nuttx-configs/px4io-v2/common/ld.script +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * configs/stm3210e-eval/nsh/ld.script - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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. - * - ****************************************************************************/ - -/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and - * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, - * FLASH memory is aliased to address 0x0000:0000 where the code expects to - * begin execution by jumping to the entry point in the 0x0800:0000 address - * range. - */ - -MEMORY -{ - flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K -} - -OUTPUT_ARCH(arm) -ENTRY(__start) /* treat __start as the anchor for dead code stripping */ -EXTERN(_vectors) /* force the vectors to be included in the output */ - -/* - * Ensure that abort() is present in the final object. The exception handling - * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). - */ -EXTERN(abort) - -SECTIONS -{ - .text : { - _stext = ABSOLUTE(.); - *(.vectors) - *(.text .text.*) - *(.fixup) - *(.gnu.warning) - *(.rodata .rodata.*) - *(.gnu.linkonce.t.*) - *(.glue_7) - *(.glue_7t) - *(.got) - *(.gcc_except_table) - *(.gnu.linkonce.r.*) - _etext = ABSOLUTE(.); - } > flash - - /* - * Init functions (static constructors and the like) - */ - .init_section : { - _sinit = ABSOLUTE(.); - KEEP(*(.init_array .init_array.*)) - _einit = ABSOLUTE(.); - } > flash - - .ARM.extab : { - *(.ARM.extab*) - } > flash - - __exidx_start = ABSOLUTE(.); - .ARM.exidx : { - *(.ARM.exidx*) - } > flash - __exidx_end = ABSOLUTE(.); - - _eronly = ABSOLUTE(.); - - /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ - - .data : { - _sdata = ABSOLUTE(.); - *(.data .data.*) - *(.gnu.linkonce.d.*) - CONSTRUCTORS - _edata = ABSOLUTE(.); - } > sram AT > flash - - .bss : { - _sbss = ABSOLUTE(.); - *(.bss .bss.*) - *(.gnu.linkonce.b.*) - *(COMMON) - _ebss = ABSOLUTE(.); - } > sram - - /* Stabs debugging sections. */ - .stab 0 : { *(.stab) } - .stabstr 0 : { *(.stabstr) } - .stab.excl 0 : { *(.stab.excl) } - .stab.exclstr 0 : { *(.stab.exclstr) } - .stab.index 0 : { *(.stab.index) } - .stab.indexstr 0 : { *(.stab.indexstr) } - .comment 0 : { *(.comment) } - .debug_abbrev 0 : { *(.debug_abbrev) } - .debug_info 0 : { *(.debug_info) } - .debug_line 0 : { *(.debug_line) } - .debug_pubnames 0 : { *(.debug_pubnames) } - .debug_aranges 0 : { *(.debug_aranges) } -} diff --git a/nuttx-configs/px4io-v2/common/setenv.sh b/nuttx-configs/px4io-v2/common/setenv.sh deleted file mode 100755 index ff9a4bf8a..000000000 --- a/nuttx-configs/px4io-v2/common/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v2/include/README.txt b/nuttx-configs/px4io-v2/include/README.txt deleted file mode 100755 index 2264a80aa..000000000 --- a/nuttx-configs/px4io-v2/include/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains header files unique to the PX4IO board. diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index bdfc4e3e2..cd2d8eba3 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -1,3 +1,170 @@ +############################################################################ +# configs/px4io-v2/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# enable precise stack overflow tracking +#INSTRUMENTATIONDEFINES = -finstrument-functions \ +# -ffixed-r10 + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = -include $(TOPDIR)/configs/px4io-v2/common/Make.defs diff --git a/nuttx-configs/px4io-v2/scripts/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx-configs/px4io-v2/scripts/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v2/src/README.txt b/nuttx-configs/px4io-v2/src/README.txt deleted file mode 100644 index d4eda82fd..000000000 --- a/nuttx-configs/px4io-v2/src/README.txt +++ /dev/null @@ -1 +0,0 @@ -This directory contains drivers unique to the STMicro STM3210E-EVAL development board. diff --git a/nuttx-configs/px4io-v2/test/Make.defs b/nuttx-configs/px4io-v2/test/Make.defs deleted file mode 100644 index 87508e22e..000000000 --- a/nuttx-configs/px4io-v2/test/Make.defs +++ /dev/null @@ -1,3 +0,0 @@ -include ${TOPDIR}/.config - -include $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/Make.defs diff --git a/nuttx-configs/px4io-v2/test/appconfig b/nuttx-configs/px4io-v2/test/appconfig deleted file mode 100644 index 3cfc41b43..000000000 --- a/nuttx-configs/px4io-v2/test/appconfig +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# configs/stm3210e-eval/nsh/appconfig -# -# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ - -# Path to example in apps/examples containing the user_start entry point - -CONFIGURED_APPS += examples/nsh - -CONFIGURED_APPS += system/readline -CONFIGURED_APPS += nshlib -CONFIGURED_APPS += reboot - -CONFIGURED_APPS += drivers/boards/px4iov2 diff --git a/nuttx-configs/px4io-v2/test/defconfig b/nuttx-configs/px4io-v2/test/defconfig deleted file mode 100755 index 132c40d80..000000000 --- a/nuttx-configs/px4io-v2/test/defconfig +++ /dev/null @@ -1,566 +0,0 @@ -############################################################################ -# configs/px4io/nsh/defconfig -# -# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# -############################################################################ -# -# architecture selection -# -# CONFIG_ARCH - identifies the arch subdirectory and, hence, the -# processor architecture. -# CONFIG_ARCH_family - for use in C code. This identifies the -# particular chip family that the architecture is implemented -# in. -# CONFIG_ARCH_architecture - for use in C code. This identifies the -# specific architecture within the chip familyl. -# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory -# CONFIG_ARCH_CHIP_name - For use in C code -# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, -# the board that supports the particular chip or SoC. -# CONFIG_ARCH_BOARD_name - for use in C code -# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) -# CONFIG_BOARD_LOOPSPERMSEC - for delay loops -# CONFIG_DRAM_SIZE - Describes the installed DRAM. -# CONFIG_DRAM_START - The start address of DRAM (physical) -# CONFIG_DRAM_END - Last address+1 of installed RAM -# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization -# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt -# stack. If defined, this symbol is the size of the interrupt -# stack in bytes. If not defined, the user task stacks will be -# used during interrupt handling. -# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions -# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. -# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. -# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. -# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that -# cause a 100 second delay during boot-up. This 100 second delay -# serves no purpose other than it allows you to calibrate -# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure -# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until -# the delay actually is 100 seconds. -# CONFIG_ARCH_DMA - Support DMA initialization -# -CONFIG_ARCH=arm -CONFIG_ARCH_ARM=y -CONFIG_ARCH_CORTEXM3=y -CONFIG_ARCH_CHIP=stm32 -CONFIG_ARCH_CHIP_STM32F100C8=y -CONFIG_ARCH_BOARD=px4iov2 -CONFIG_ARCH_BOARD_PX4IO_V2=y -CONFIG_BOARD_LOOPSPERMSEC=24000 -CONFIG_DRAM_SIZE=0x00002000 -CONFIG_DRAM_START=0x20000000 -CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) -CONFIG_ARCH_IRQPRIO=y -CONFIG_ARCH_INTERRUPTSTACK=n -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARCH_BOOTLOADER=n -CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=y -CONFIG_ARCH_CALIBRATION=n -CONFIG_ARCH_DMA=n -CONFIG_ARMV7M_CMNVECTOR=y - -# -# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): -# -# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG -# -# JTAG Enable options: -# -# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) -# but without JNTRST. -# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled -# -CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y -CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n - -# -# Individual subsystems can be enabled: -# AHB: -CONFIG_STM32_DMA1=n -CONFIG_STM32_DMA2=n -CONFIG_STM32_CRC=n -# APB1: -# Timers 2,3 and 4 are owned by the PWM driver -CONFIG_STM32_TIM2=n -CONFIG_STM32_TIM3=n -CONFIG_STM32_TIM4=n -CONFIG_STM32_TIM5=n -CONFIG_STM32_TIM6=n -CONFIG_STM32_TIM7=n -CONFIG_STM32_WWDG=n -CONFIG_STM32_SPI2=n -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=n -CONFIG_STM32_BKP=n -CONFIG_STM32_PWR=n -CONFIG_STM32_DAC=n -# APB2: -CONFIG_STM32_ADC1=y -CONFIG_STM32_ADC2=n -# TIM1 is owned by the HRT -CONFIG_STM32_TIM1=n -CONFIG_STM32_SPI1=n -CONFIG_STM32_TIM8=n -CONFIG_STM32_USART1=y -CONFIG_STM32_ADC3=n - -# -# Timer and I2C devices may need to the following to force power to be applied: -# -#CONFIG_STM32_FORCEPOWER=y - -# -# STM32F100 specific serial device driver settings -# -# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the -# console and ttys0 (default is the USART1). -# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. -# This specific the size of the receive buffer -# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before -# being sent. This specific the size of the transmit buffer -# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be -# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. -# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity -# CONFIG_USARTn_2STOP - Two stop bits -# -CONFIG_USART1_SERIAL_CONSOLE=y -CONFIG_USART2_SERIAL_CONSOLE=n -CONFIG_USART3_SERIAL_CONSOLE=n - -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 -CONFIG_USART3_TXBUFSIZE=64 - -CONFIG_USART1_RXBUFSIZE=64 -CONFIG_USART2_RXBUFSIZE=64 -CONFIG_USART3_RXBUFSIZE=64 - -CONFIG_USART1_BAUD=57600 -CONFIG_USART2_BAUD=115200 -CONFIG_USART3_BAUD=115200 - -CONFIG_USART1_BITS=8 -CONFIG_USART2_BITS=8 -CONFIG_USART3_BITS=8 - -CONFIG_USART1_PARITY=0 -CONFIG_USART2_PARITY=0 -CONFIG_USART3_PARITY=0 - -CONFIG_USART1_2STOP=0 -CONFIG_USART2_2STOP=0 -CONFIG_USART3_2STOP=0 - -# -# PX4IO specific driver settings -# -# CONFIG_HRT_TIMER -# Enables the high-resolution timer. The board definition must -# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ -# compare channels to be used. -# CONFIG_HRT_PPM -# Enables R/C PPM input using the HRT. The board definition must -# set HRT_PPM_CHANNEL to the timer capture/compare channel to be -# used, and define GPIO_PPM_IN to configure the appropriate timer -# GPIO. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# -# -CONFIG_HRT_TIMER=y -CONFIG_HRT_PPM=y - -# -# General build options -# -# CONFIG_RRLOAD_BINARY - make the rrload binary format used with -# BSPs from www.ridgerun.com using the tools/mkimage.sh script -# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format -# used with many different loaders using the GNU objcopy program -# Should not be selected if you are not using the GNU toolchain. -# CONFIG_RAW_BINARY - make a raw binary format file used with many -# different loaders using the GNU objcopy program. This option -# should not be selected if you are not using the GNU toolchain. -# CONFIG_HAVE_LIBM - toolchain supports libm.a -# -CONFIG_RRLOAD_BINARY=n -CONFIG_INTELHEX_BINARY=n -CONFIG_MOTOROLA_SREC=n -CONFIG_RAW_BINARY=y -CONFIG_HAVE_LIBM=n - -# -# General OS setup -# -# CONFIG_APPS_DIR - Identifies the relative path to the directory -# that builds the application to link with NuttX. Default: ../apps -# CONFIG_DEBUG - enables built-in debug options -# CONFIG_DEBUG_VERBOSE - enables verbose debug output -# CONFIG_DEBUG_SYMBOLS - build without optimization and with -# debug symbols (needed for use with a debugger). -# CONFIG_HAVE_CXX - Enable support for C++ -# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support -# for initialization of static C++ instances for this architecture -# and for the selected toolchain (via up_cxxinitialize()). -# CONFIG_MM_REGIONS - If the architecture includes multiple -# regions of memory to allocate from, this specifies the -# number of memory regions that the memory manager must -# handle and enables the API mm_addregion(start, end); -# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot -# time console output -# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz -# or MSEC_PER_TICK=10. This setting may be defined to -# inform NuttX that the processor hardware is providing -# system timer interrupts at some interrupt interval other -# than 10 msec. -# CONFIG_RR_INTERVAL - The round robin timeslice will be set -# this number of milliseconds; Round robin scheduling can -# be disabled by setting this value to zero. -# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in -# scheduler to monitor system performance -# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a -# task name to save in the TCB. Useful if scheduler -# instrumentation is selected. Set to zero to disable. -# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - -# Used to initialize the internal time logic. -# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. -# You would only need this if you are concerned about accurate -# time conversions in the past or in the distant future. -# CONFIG_JULIAN_TIME - Enables Julian time conversions. You -# would only need this if you are concerned about accurate -# time conversion in the distand past. You must also define -# CONFIG_GREGORIAN_TIME in order to use Julian time. -# CONFIG_DEV_CONSOLE - Set if architecture-specific logic -# provides /dev/console. Enables stdout, stderr, stdin. -# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console -# driver (minimul support) -# CONFIG_MUTEX_TYPES: Set to enable support for recursive and -# errorcheck mutexes. Enables pthread_mutexattr_settype(). -# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority -# inheritance on mutexes and semaphores. -# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority -# inheritance is enabled. It defines the maximum number of -# different threads (minus one) that can take counts on a -# semaphore with priority inheritance support. This may be -# set to zero if priority inheritance is disabled OR if you -# are only using semaphores as mutexes (only one holder) OR -# if no more than two threads participate using a counting -# semaphore. -# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, -# then this setting is the maximum number of higher priority -# threads (minus 1) than can be waiting for another thread -# to release a count on a semaphore. This value may be set -# to zero if no more than one thread is expected to wait for -# a semaphore. -# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors -# by task_create() when a new task is started. If set, all -# files/drivers will appear to be closed in the new task. -# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first -# three file descriptors (stdin, stdout, stderr) by task_create() -# when a new task is started. If set, all files/drivers will -# appear to be closed in the new task except for stdin, stdout, -# and stderr. -# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket -# desciptors by task_create() when a new task is started. If -# set, all sockets will appear to be closed in the new task. -# CONFIG_NXFLAT. Enable support for the NXFLAT binary format. -# This format will support execution of NuttX binaries located -# in a ROMFS filesystem (see examples/nxflat). -# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to -# handle delayed processing from interrupt handlers. This feature -# is required for some drivers but, if there are not complaints, -# can be safely disabled. The worker thread also performs -# garbage collection -- completing any delayed memory deallocations -# from interrupt handlers. If the worker thread is disabled, -# then that clean will be performed by the IDLE thread instead -# (which runs at the lowest of priority and may not be appropriate -# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE -# is enabled, then the following options can also be used: -# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker -# thread. Default: 50 -# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for -# work in units of microseconds. Default: 50*1000 (50 MS). -# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker -# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. -# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up -# the worker thread. Default: 4 -# -#CONFIG_APPS_DIR= -CONFIG_DEBUG=n -CONFIG_DEBUG_VERBOSE=n -CONFIG_DEBUG_SYMBOLS=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=n -CONFIG_MM_REGIONS=1 -CONFIG_MM_SMALL=y -CONFIG_ARCH_LOWPUTC=y -CONFIG_RR_INTERVAL=200 -CONFIG_SCHED_INSTRUMENTATION=n -CONFIG_TASK_NAME_SIZE=0 -CONFIG_START_YEAR=2009 -CONFIG_START_MONTH=9 -CONFIG_START_DAY=21 -CONFIG_GREGORIAN_TIME=n -CONFIG_JULIAN_TIME=n -CONFIG_DEV_CONSOLE=y -CONFIG_DEV_LOWCONSOLE=n -CONFIG_MUTEX_TYPES=n -CONFIG_PRIORITY_INHERITANCE=n -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SEM_NNESTPRIO=0 -CONFIG_FDCLONE_DISABLE=n -CONFIG_FDCLONE_STDIO=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_NXFLAT=n -CONFIG_SCHED_WORKQUEUE=n -CONFIG_SCHED_WORKPRIORITY=50 -CONFIG_SCHED_WORKPERIOD=(50*1000) -CONFIG_SCHED_WORKSTACKSIZE=512 -CONFIG_SIG_SIGWORK=4 - -CONFIG_USER_ENTRYPOINT="nsh_main" -# -# The following can be used to disable categories of -# APIs supported by the OS. If the compiler supports -# weak functions, then it should not be necessary to -# disable functions unless you want to restrict usage -# of those APIs. -# -# There are certain dependency relationships in these -# features. -# -# o mq_notify logic depends on signals to awaken tasks -# waiting for queues to become full or empty. -# o pthread_condtimedwait() depends on signals to wake -# up waiting tasks. -# -CONFIG_DISABLE_CLOCK=n -CONFIG_DISABLE_POSIX_TIMERS=y -CONFIG_DISABLE_PTHREAD=n -CONFIG_DISABLE_SIGNALS=n -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_MOUNTPOINT=y -CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y - -# -# Misc libc settings -# -# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a -# little smaller if we do not support fieldwidthes -# -CONFIG_NOPRINTF_FIELDWIDTH=n - -# -# Allow for architecture optimized implementations -# -# The architecture can provide optimized versions of the -# following to improve system performance -# -CONFIG_ARCH_MEMCPY=n -CONFIG_ARCH_MEMCMP=n -CONFIG_ARCH_MEMMOVE=n -CONFIG_ARCH_MEMSET=n -CONFIG_ARCH_STRCMP=n -CONFIG_ARCH_STRCPY=n -CONFIG_ARCH_STRNCPY=n -CONFIG_ARCH_STRLEN=n -CONFIG_ARCH_STRNLEN=n -CONFIG_ARCH_BZERO=n - -# -# Sizes of configurable things (0 disables) -# -# CONFIG_MAX_TASKS - The maximum number of simultaneously -# active tasks. This value must be a power of two. -# CONFIG_MAX_TASK_ARGS - This controls the maximum number of -# of parameters that a task may receive (i.e., maxmum value -# of 'argc') -# CONFIG_NPTHREAD_KEYS - The number of items of thread- -# specific data that can be retained -# CONFIG_NFILE_DESCRIPTORS - The maximum number of file -# descriptors (one for each open) -# CONFIG_NFILE_STREAMS - The maximum number of streams that -# can be fopen'ed -# CONFIG_NAME_MAX - The maximum size of a file name. -# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate -# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_NUNGET_CHARS - Number of characters that can be -# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) -# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message -# structures. The system manages a pool of preallocated -# message structures to minimize dynamic allocations -# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with -# a fixed payload size given by this settin (does not include -# other message structure overhead. -# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that -# can be passed to a watchdog handler -# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog -# structures. The system manages a pool of preallocated -# watchdog structures to minimize dynamic allocations -# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX -# timer structures. The system manages a pool of preallocated -# timer structures to minimize dynamic allocations. Set to -# zero for all dynamic allocations. -# -CONFIG_MAX_TASKS=4 -CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=2 -CONFIG_NFILE_DESCRIPTORS=6 -CONFIG_NFILE_STREAMS=4 -CONFIG_NAME_MAX=32 -CONFIG_STDIO_BUFFER_SIZE=64 -CONFIG_NUNGET_CHARS=2 -CONFIG_PREALLOC_MQ_MSGS=1 -CONFIG_MQ_MAXMSGSIZE=32 -CONFIG_MAX_WDOGPARMS=2 -CONFIG_PREALLOC_WDOGS=3 -CONFIG_PREALLOC_TIMERS=1 - - -# -# Settings for apps/nshlib -# -# CONFIG_NSH_BUILTIN_APPS - Support external registered, -# "named" applications that can be executed from the NSH -# command line (see apps/README.txt for more information). -# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer -# CONFIG_NSH_STRERROR - Use strerror(errno) -# CONFIG_NSH_LINELEN - Maximum length of one command line -# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi -# CONFIG_NSH_DISABLESCRIPT - Disable scripting support -# CONFIG_NSH_DISABLEBG - Disable background commands -# CONFIG_NSH_ROMFSETC - Use startup script in /etc -# CONFIG_NSH_CONSOLE - Use serial console front end -# CONFIG_NSH_TELNET - Use telnetd console front end -# CONFIG_NSH_ARCHINIT - Platform provides architecture -# specific initialization (nsh_archinitialize()). -# -# If CONFIG_NSH_TELNET is selected: -# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size -# CONFIG_NSH_DHCPC - Obtain address using DHCP -# CONFIG_NSH_IPADDR - Provides static IP address -# CONFIG_NSH_DRIPADDR - Provides static router IP address -# CONFIG_NSH_NETMASK - Provides static network mask -# CONFIG_NSH_NOMAC - Use a bogus MAC address -# -# If CONFIG_NSH_ROMFSETC is selected: -# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint -# CONFIG_NSH_INITSCRIPT - Relative path to init script -# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor -# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size -# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor -# CONFIG_NSH_FATSECTSIZE - FAT FS sector size -# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors -# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint -# -CONFIG_BUILTIN=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_FILEIOSIZE=64 -CONFIG_NSH_STRERROR=n -CONFIG_NSH_LINELEN=64 -CONFIG_NSH_NESTDEPTH=1 -CONFIG_NSH_DISABLESCRIPT=y -CONFIG_NSH_DISABLEBG=n -CONFIG_NSH_ROMFSETC=n -CONFIG_NSH_CONSOLE=y -CONFIG_NSH_TELNET=n -CONFIG_NSH_ARCHINIT=n -CONFIG_NSH_IOBUFFER_SIZE=256 -CONFIG_NSH_STACKSIZE=1024 -CONFIG_NSH_DHCPC=n -CONFIG_NSH_NOMAC=n -CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) -CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) -CONFIG_NSH_NETMASK=(255<<24|255<<16|255<<8|0) -CONFIG_NSH_ROMFSMOUNTPT="/etc" -CONFIG_NSH_INITSCRIPT="init.d/rcS" -CONFIG_NSH_ROMFSDEVNO=0 -CONFIG_NSH_ROMFSSECTSIZE=64 -CONFIG_NSH_FATDEVNO=1 -CONFIG_NSH_FATSECTSIZE=512 -CONFIG_NSH_FATNSECTORS=1024 -CONFIG_NSH_FATMOUNTPT=/tmp - -# -# Architecture-specific NSH options -# -CONFIG_NSH_MMCSDSPIPORTNO=0 -CONFIG_NSH_MMCSDSLOTNO=0 -CONFIG_NSH_MMCSDMINOR=0 - -# -# Stack and heap information -# -# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP -# operation from FLASH but must copy initialized .data sections to RAM. -# (should also be =n for the STM3210E-EVAL which always runs from flash) -# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH -# but copy themselves entirely into RAM for better performance. -# CONFIG_CUSTOM_STACK - The up_ implementation will handle -# all stack operations outside of the nuttx model. -# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) -# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. -# This is the thread that (1) performs the inital boot of the system up -# to the point where user_start() is spawned, and (2) there after is the -# IDLE thread that executes only when there is no other thread ready to -# run. -# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate -# for the main user thread that begins at the user_start() entry point. -# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size -# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size -# CONFIG_HEAP_BASE - The beginning of the heap -# CONFIG_HEAP_SIZE - The size of the heap -# -CONFIG_BOOT_RUNFROMFLASH=n -CONFIG_BOOT_COPYTORAM=n -CONFIG_CUSTOM_STACK=n -CONFIG_STACK_POINTER= -CONFIG_IDLETHREAD_STACKSIZE=800 -CONFIG_USERMAIN_STACKSIZE=1024 -CONFIG_PTHREAD_STACK_MIN=256 -CONFIG_PTHREAD_STACK_DEFAULT=512 -CONFIG_HEAP_BASE= -CONFIG_HEAP_SIZE= diff --git a/nuttx-configs/px4io-v2/test/setenv.sh b/nuttx-configs/px4io-v2/test/setenv.sh deleted file mode 100755 index d83685192..000000000 --- a/nuttx-configs/px4io-v2/test/setenv.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -# configs/stm3210e-eval/dfu/setenv.sh -# -# Copyright (C) 2009 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt -# -# 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. -# - -if [ "$(basename $0)" = "setenv.sh" ] ; then - echo "You must source this script, not run it!" 1>&2 - exit 1 -fi - -if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi - -WD=`pwd` -export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" -export BUILDROOT_BIN="${WD}/../buildroot/build_arm_nofpu/staging_dir/bin" -export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" - -echo "PATH : ${PATH}" -- cgit v1.2.3 From 4708693f86858772e88d7736a4996023d4f57327 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Aug 2013 18:54:03 +1000 Subject: nshterm: start a nsh console on any device this is used in APM startup to fallback to a console on ttyACM0 if startup fails for any reason --- src/systemcmds/nshterm/module.mk | 41 ++++++++++++++++++ src/systemcmds/nshterm/nshterm.c | 90 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 131 insertions(+) create mode 100644 src/systemcmds/nshterm/module.mk create mode 100644 src/systemcmds/nshterm/nshterm.c diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk new file mode 100644 index 000000000..a48588535 --- /dev/null +++ b/src/systemcmds/nshterm/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 nshterm utility +# + +MODULE_COMMAND = nshterm +SRCS = nshterm.c + +MODULE_STACKSIZE = 3000 diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c new file mode 100644 index 000000000..bde0e7841 --- /dev/null +++ b/src/systemcmds/nshterm/nshterm.c @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Andrew Tridgell + * + * 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 nshterm.c + * start a nsh terminal on a given port. This can be useful for error + * handling in startup scripts to start a nsh shell on /dev/ttyACM0 + * for diagnostics + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int nshterm_main(int argc, char *argv[]); + +int +nshterm_main(int argc, char *argv[]) +{ + if (argc < 2) { + printf("Usage: nshterm \n"); + exit(1); + } + uint8_t retries = 0; + int fd = -1; + while (retries < 5) { + // the retries are to cope with the behaviour of /dev/ttyACM0 + // which may not be ready immediately. + fd = open(argv[1], O_RDWR); + if (fd != -1) { + break; + } + usleep(100000); + retries++; + } + if (fd == -1) { + perror(argv[1]); + exit(1); + } + // setup standard file descriptors + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; +} -- cgit v1.2.3 From 8e599e4a3cf1d89c5ce4d1e8f1020fe047e0a55c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Aug 2013 12:39:40 +1000 Subject: Merged USB ID changes to match bootloader --- nuttx-configs/px4fmu-v2/nsh/defconfig | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2e73a5a07..fcc7c7df9 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -652,10 +652,12 @@ CONFIG_USBDEV_MAXPOWER=500 CONFIG_CDCACM=y CONFIG_CDCACM_CONSOLE=y CONFIG_CDCACM_EP0MAXPACKET=64 -CONFIG_CDCACM_EPINTIN=1 +# the endpoint addresses are chosen to match the +# bootloader for the FMUv2 +CONFIG_CDCACM_EPINTIN=3 CONFIG_CDCACM_EPINTIN_FSSIZE=64 CONFIG_CDCACM_EPINTIN_HSSIZE=64 -CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT=1 CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 CONFIG_CDCACM_EPBULKIN=2 @@ -664,12 +666,15 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 +# these IDs are chosen to match the bootloader, to prevent +# windows from seeing it as a new device when switching from +# bootloader mode to flight mode CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTID=0x0016 CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2,0" # CONFIG_USBMSC is not set # CONFIG_USBHOST is not set # CONFIG_WIRELESS is not set -- cgit v1.2.3 From 6cf24ac106688d70bdeda9d13fa252246f599b5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 00:16:59 +0200 Subject: Increased comm buf size to better deal with higher-speed MAVLink transfers --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index eb225e22c..dc884413a 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -559,8 +559,8 @@ CONFIG_CDCACM_EPBULKIN_FSSIZE=64 CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" -- cgit v1.2.3 From 9499e48a524941d955c1b092c7b4e8ebdc1cc807 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 01:52:22 +0200 Subject: Fixed setting mag queue depth --- src/drivers/lsm303d/lsm303d.cpp | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index d9801f88f..7a65f644a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -811,8 +811,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - case SENSORIOCSQUEUEDEPTH: + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct mag_report *buf = new struct mag_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _mag_reports; + _num_mag_reports = arg; + _mag_reports = buf; + start(); + + return OK; + } + case SENSORIOCGQUEUEDEPTH: + return _num_mag_reports - 1; + case SENSORIOCRESET: return ioctl(filp, cmd, arg); -- cgit v1.2.3 From fbd5aae8c67ef7695cc31aa3c9d450a3e0ce46cb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 3 Aug 2013 19:41:37 -0700 Subject: Revert "Merged USB ID changes to match bootloader" This reverts commit 8e599e4a3cf1d89c5ce4d1e8f1020fe047e0a55c. --- nuttx-configs/px4fmu-v2/nsh/defconfig | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index fcc7c7df9..2e73a5a07 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -652,12 +652,10 @@ CONFIG_USBDEV_MAXPOWER=500 CONFIG_CDCACM=y CONFIG_CDCACM_CONSOLE=y CONFIG_CDCACM_EP0MAXPACKET=64 -# the endpoint addresses are chosen to match the -# bootloader for the FMUv2 -CONFIG_CDCACM_EPINTIN=3 +CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 CONFIG_CDCACM_EPINTIN_HSSIZE=64 -CONFIG_CDCACM_EPBULKOUT=1 +CONFIG_CDCACM_EPBULKOUT=3 CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 CONFIG_CDCACM_EPBULKIN=2 @@ -666,15 +664,12 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 -# these IDs are chosen to match the bootloader, to prevent -# windows from seeing it as a new device when switching from -# bootloader mode to flight mode +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0016 +CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2,0" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" # CONFIG_USBMSC is not set # CONFIG_USBHOST is not set # CONFIG_WIRELESS is not set -- cgit v1.2.3 From e931d3b9cda723d63bf352ca866dc499d8df21f5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 3 Aug 2013 22:35:18 -0700 Subject: Add an option to the systemreset() call and to the reboot command (-b) to reboot into the bootloader. The system will remain in the bootloader until it's reset, or until an upload is completed. --- src/modules/commander/state_machine_helper.c | 2 +- src/modules/systemlib/systemlib.c | 12 +++++++++++- src/modules/systemlib/systemlib.h | 2 +- src/systemcmds/reboot/reboot.c | 19 ++++++++++++++++++- 4 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index c26478785..9b6527c33 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -141,7 +141,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); usleep(500000); - systemreset(); + systemreset(false); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 96276b56a..57a751e1c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -50,9 +50,19 @@ #include #include +#include + #include "systemlib.h" -__EXPORT extern void systemreset(void) { +void +systemreset(bool to_bootloader) +{ + if (to_bootloader) { + stm32_pwr_enablebkp(); + + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xb007b007; + } up_systemreset(); } diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 77fdfe08a..3728f2067 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -45,7 +45,7 @@ __BEGIN_DECLS /** Reboots the board */ -__EXPORT void systemreset(void) noreturn_function; +__EXPORT void systemreset(bool to_bootloader) noreturn_function; /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 0fd1e2724..91a2c2eb8 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -40,14 +40,31 @@ #include #include #include +#include #include +#include __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - systemreset(); + int ch; + bool to_bootloader = false; + + while ((ch = getopt(argc, argv, "b")) != -1) { + switch (ch) { + case 'b': + to_bootloader = true; + break; + default: + errx(1, "usage: reboot [-b]\n" + " -b reboot into the bootloader"); + + } + } + + systemreset(to_bootloader); } -- cgit v1.2.3 From 7ddd88623efcdc83eeb93bbb592b936ac75096f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:09:16 +1000 Subject: mathlib: added LowPassFilter2p object used in gyro and accel drivers for better filtering --- .../mathlib/math/filter/LowPassFilter2p.cpp | 77 +++++++++++++++++++++ .../mathlib/math/filter/LowPassFilter2p.hpp | 78 ++++++++++++++++++++++ src/modules/mathlib/math/filter/module.mk | 44 ++++++++++++ 3 files changed, 199 insertions(+) create mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.cpp create mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.hpp create mode 100644 src/modules/mathlib/math/filter/module.mk diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp new file mode 100644 index 000000000..efb17225d --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * 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 LowPassFilter.cpp +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall + +#include "LowPassFilter2p.hpp" +#include "math.h" + +namespace math +{ + +void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) +{ + _cutoff_freq = cutoff_freq; + float fr = sample_freq/_cutoff_freq; + float ohm = tanf(M_PI_F/fr); + float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; + _b0 = ohm*ohm/c; + _b1 = 2.0f*_b0; + _b2 = _b0; + _a1 = 2.0f*(ohm*ohm-1.0f)/c; + _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; +} + +float LowPassFilter2p::apply(float sample) +{ + // do the filtering + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; + if (isnan(delay_element_0) || isinf(delay_element_0)) { + // don't allow bad values to propogate via the filter + delay_element_0 = sample; + } + float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + // return the value. Should be no need to check limits + return output; +} + +} // namespace math + diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp new file mode 100644 index 000000000..208ec98d4 --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * 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 LowPassFilter.h +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall +/// Adapted for PX4 by Andrew Tridgell + +#pragma once + +namespace math +{ +class __EXPORT LowPassFilter2p +{ +public: + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + _delay_element_1 = _delay_element_2 = 0; + } + + // change parameters + void set_cutoff_frequency(float sample_freq, float cutoff_freq); + + // apply - Add a new raw value to the filter + // and retrieve the filtered result + float apply(float sample); + + // return the cutoff frequency + float get_cutoff_freq(void) const { + return _cutoff_freq; + } + +private: + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 +}; + +} // namespace math diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk new file mode 100644 index 000000000..fe92c8c70 --- /dev/null +++ b/src/modules/mathlib/math/filter/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. +# +############################################################################ + +# +# filter library +# +SRCS = LowPassFilter2p.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config -- cgit v1.2.3 From a87690d0e279bfe273465dc34ad0058bdaabd015 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:10:01 +1000 Subject: Added L3GD20 lowpass --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 3bb9a7764..9b4bda0ae 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -63,6 +63,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -188,6 +189,10 @@ private: perf_counter_t _sample_perf; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -278,7 +283,10 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(250, 30), + _gyro_filter_y(250, 30), + _gyro_filter_z(250, 30) { // enable debug() calls _debug_enabled = true; @@ -441,6 +449,13 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; + // adjust filters + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* if we need to start the poll state machine, do it */ if (want_start) start(); @@ -493,10 +508,17 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGSAMPLERATE: return _current_rate; - case GYROIOCSLOWPASS: + case GYROIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } + case GYROIOCGLOWPASS: - /* XXX not implemented due to wacky interaction with samplerate */ - return -EINVAL; + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -699,6 +721,11 @@ L3GD20::measure() report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + report->x = _gyro_filter_x.apply(report->x); + report->y = _gyro_filter_y.apply(report->y); + report->z = _gyro_filter_z.apply(report->z); + report->scaling = _gyro_range_scale; report->range_rad_s = _gyro_range_rad_s; -- cgit v1.2.3 From 686edfefb8b8bb90e630cac166ad06776229f55a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 13:46:30 +1000 Subject: Removed LSM303D filter --- src/drivers/lsm303d/iirFilter.c | 255 ---------------------------------------- src/drivers/lsm303d/iirFilter.h | 59 ---------- src/drivers/lsm303d/lsm303d.cpp | 22 ---- src/drivers/lsm303d/module.mk | 5 +- 4 files changed, 3 insertions(+), 338 deletions(-) delete mode 100644 src/drivers/lsm303d/iirFilter.c delete mode 100644 src/drivers/lsm303d/iirFilter.h diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c deleted file mode 100644 index 8311f14d6..000000000 --- a/src/drivers/lsm303d/iirFilter.c +++ /dev/null @@ -1,255 +0,0 @@ -#include "math.h" -#include "string.h" -#include "iirFilter.h" - -/////////////////////////////////////////////////////////////////////////////// -// Internal function prototypes - -int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd); - -int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd); - -int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt); - -int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly); - -/////////////////////////////////////////////////////////////////////////////// -// external functions - -int testFunction() -{ - printf("TEST\n"); - return 1; -} - -int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt) -{ - TF_POLY_t polyd; - TF_ZPG_t zpgd; - - memset(pFilt, 0, sizeof(FIL_T)); - - // perform bilinear transform with frequency pre warping - btDifcToZpgd(pDifc, Ts, &zpgd); - - // calculate polynom - tZpgxToPolyx(&zpgd, &polyd); - - // set the filter parameters - tPolydToFil(&polyd, pFilt); - - return 1; -} - -// run filter using inp, return output of the filter -float updateFilter(FIL_T *pFilt, float inp) -{ - float outp = 0; - int idx; // index used for different things - int i; // loop variable - - // Store the input to the input array - idx = pFilt->inpCnt % MAX_LENGTH; - pFilt->inpData[idx] = inp; - - // calculate numData * inpData - for (i = 0; i < pFilt->numLength; i++) - { - // index of inp array - idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH; - outp += pFilt->numData[i] * pFilt->inpData[idx]; - } - - // calculate denData * outData - for (i = 0; i < pFilt->denLength; i++) - { - // index of inp array - idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH; - outp -= pFilt->denData[i] * pFilt->outData[idx]; - } - - // store the ouput data to the output array - idx = pFilt->inpCnt % MAX_LENGTH; - pFilt->outData[idx] = outp; - - pFilt->inpCnt += 1; - - return outp; -} - -/////////////////////////////////////////////////////////////////////////////// -// Internal functions - -int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt) -{ - double gain; - int i; - - if (pkPolyd->Ts < 0) - { - return 0; - } - - // initialize filter to 0 - memset(pFilt, 0, sizeof(FIL_T)); - - gain = pkPolyd->denData[pkPolyd->denLength - 1]; - pFilt->Ts = pkPolyd->Ts; - - pFilt->denLength = pkPolyd->denLength - 1; - pFilt->numLength = pkPolyd->numLength; - - for (i = 0; i < pkPolyd->numLength; i++) - { - pFilt->numData[i] = pkPolyd->numData[i] / gain; - } - - for (i = 0; i < (pkPolyd->denLength - 1); i++) - { - pFilt->denData[i] = pkPolyd->denData[i] / gain; - } -} - -// bilinear transformation of poles and zeros -int btDifcToZpgd(const TF_DIF_t *pkDifc, - double Ts, - TF_ZPG_t *pZpgd) -{ - TF_ZPG_t zpgc; - int totDiff; - int i; - - zpgc.zerosLength = 0; - zpgc.polesLength = 0; - zpgc.gain = pkDifc->gain; - zpgc.Ts = pkDifc->Ts; - - // Total number of differentiators / integerators - // positive diff, negative integ, 0 for no int/diff - totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength; - - while (zpgc.zerosLength < totDiff) - { - zpgc.zerosData[zpgc.zerosLength] = 0; - zpgc.zerosLength++; - } - while (zpgc.polesLength < -totDiff) - { - zpgc.polesData[zpgc.polesLength] = 0; - zpgc.polesLength++; - } - - // The next step is to calculate the poles - // This has to be done for the highpass and lowpass filters - // As we are using bilinear transformation there will be frequency - // warping which has to be accounted for - for (i = 0; i < pkDifc->lowpassLength; i++) - { - // pre-warping: - double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0); - // calculate pole: - zpgc.polesData[zpgc.polesLength] = -freq; - // adjust gain such that lp has gain = 1 for low frequencies - zpgc.gain *= freq; - zpgc.polesLength++; - } - for (i = 0; i < pkDifc->highpassLength; i++) - { - // pre-warping - double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0); - // calculate pole: - zpgc.polesData[zpgc.polesLength] = -freq; - // gain does not have to be adjusted - zpgc.polesLength++; - } - - return btZpgcToZpgd(&zpgc, Ts, pZpgd); -} - -// bilinear transformation of poles and zeros -int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, - double Ts, - TF_ZPG_t *pZpgd) -{ - int i; - - // init digital gain - pZpgd->gain = pkZpgc->gain; - - // transform the poles - pZpgd->polesLength = pkZpgc->polesLength; - for (i = 0; i < pkZpgc->polesLength; i++) - { - pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]); - pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]); - } - - // transform the zeros - pZpgd->zerosLength = pkZpgc->zerosLength; - for (i = 0; i < pkZpgc->zerosLength; i++) - { - pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]); - pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]); - } - - // if we don't have the same number of poles as zeros we have to add - // poles or zeros due to the bilinear transformation - while (pZpgd->zerosLength < pZpgd->polesLength) - { - pZpgd->zerosData[pZpgd->zerosLength] = -1.0; - pZpgd->zerosLength++; - } - while (pZpgd->zerosLength > pZpgd->polesLength) - { - pZpgd->polesData[pZpgd->polesLength] = -1.0; - pZpgd->polesLength++; - } - - pZpgd->Ts = Ts; - - return 1; -} - -// calculate polynom from zero, pole, gain form -int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly) -{ - int i, j; // counter variable - double tmp0, tmp1, gain; - - // copy Ts - pPoly->Ts = pkZpg->Ts; - - // calculate denominator polynom - pPoly->denLength = 1; - pPoly->denData[0] = 1.0; - for (i = 0; i < pkZpg->polesLength; i++) - { - // init temporary variable - tmp0 = 0.0; - // increase the polynom by 1 - pPoly->denData[pPoly->denLength] = 0; - pPoly->denLength++; - for (j = 0; j < pPoly->denLength; j++) - { - tmp1 = pPoly->denData[j]; - pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0; - tmp0 = tmp1; - } - } - - // calculate numerator polynom - pPoly->numLength = 1; - pPoly->numData[0] = pkZpg->gain; - for (i = 0; i < pkZpg->zerosLength; i++) - { - tmp0 = 0.0; - pPoly->numData[pPoly->numLength] = 0; - pPoly->numLength++; - for (j = 0; j < pPoly->numLength; j++) - { - tmp1 = pPoly->numData[j]; - pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0; - tmp0 = tmp1; - } - } -} diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h deleted file mode 100644 index ab4223a8e..000000000 --- a/src/drivers/lsm303d/iirFilter.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef IIRFILTER__H__ -#define IIRFILTER__H__ - -__BEGIN_DECLS - -#define MAX_LENGTH 4 - -typedef struct FILTER_s -{ - float denData[MAX_LENGTH]; - float numData[MAX_LENGTH]; - int denLength; - int numLength; - float Ts; - float inpData[MAX_LENGTH]; - float outData[MAX_LENGTH]; - unsigned int inpCnt; -} FIL_T; - -typedef struct TF_ZPG_s -{ - int zerosLength; - double zerosData[MAX_LENGTH + 1]; - int polesLength; - double polesData[MAX_LENGTH + 1]; - double gain; - double Ts; -} TF_ZPG_t; - -typedef struct TF_POLY_s -{ - int numLength; - double numData[MAX_LENGTH]; - int denLength; - double denData[MAX_LENGTH]; - double Ts; -} TF_POLY_t; - -typedef struct TF_DIF_s -{ - int numInt; - int numDiff; - int lowpassLength; - int highpassLength; - double lowpassData[MAX_LENGTH]; - int highpassData[MAX_LENGTH]; - double gain; - double Ts; -} TF_DIF_t; - -__EXPORT int testFunction(void); - -__EXPORT float updateFilter(FIL_T *pFilt, float inp); - -__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt); - -__END_DECLS - -#endif diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 7a65f644a..9ebffcac9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,8 +65,6 @@ #include -#include "iirFilter.h" - /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -221,10 +219,6 @@ private: unsigned _current_samplerate; -// FIL_T _filter_x; -// FIL_T _filter_y; -// FIL_T _filter_z; - unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -494,22 +488,6 @@ LSM303D::init() set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ - /* initiate filter */ -// TF_DIF_t tf_filter; -// tf_filter.numInt = 0; -// tf_filter.numDiff = 0; -// tf_filter.lowpassLength = 2; -// tf_filter.highpassLength = 0; -// tf_filter.lowpassData[0] = 10; -// tf_filter.lowpassData[1] = 10; -// //tf_filter.highpassData[0] = ; -// tf_filter.gain = 1; -// tf_filter.Ts = 1/_current_samplerate; -// -// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); -// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); -// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); - mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index 8fd5679c9..e40f718c5 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -3,5 +3,6 @@ # MODULE_COMMAND = lsm303d -SRCS = lsm303d.cpp \ - iirFilter.c +SRCS = lsm303d.cpp + + -- cgit v1.2.3 From 26204496b69740bddfd6f8ddbdd71ee4e755008c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Aug 2013 15:10:42 +1000 Subject: Merged LSM303D lowpass --- src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++----------------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 9ebffcac9..56400c843 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -64,6 +64,7 @@ #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -232,6 +233,10 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + /** * Start automatic measurement. */ @@ -402,7 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_range_ga(0.0f), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _accel_filter_x(800, 30), + _accel_filter_y(800, 30), + _accel_filter_z(800, 30) { // enable debug() calls _debug_enabled = true; @@ -446,7 +454,6 @@ LSM303D::init() { int ret = ERROR; int mag_ret; - int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) @@ -622,7 +629,6 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: @@ -645,6 +651,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust sample rate of sensor */ set_samplerate(arg); + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _accel_call.period = _call_accel_interval = ticks; @@ -695,15 +708,17 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX implement */ return -EINVAL; -// case ACCELIOCSLOWPASS: - case ACCELIOCGLOWPASS: + case ACCELIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_accel_interval; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } - unsigned bandwidth; - - if (OK == get_antialias_filter_bandwidth(bandwidth)) - return bandwidth; - else - return -EINVAL; + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSSCALE: { @@ -1186,17 +1201,13 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; -// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; -// float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; -// float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; -// -// accel_report->x = updateFilter(&_filter_x, x_in_new); -// accel_report->y = updateFilter(&_filter_y, y_in_new); -// accel_report->z = updateFilter(&_filter_z, z_in_new); - - accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + accel_report->x = _accel_filter_x.apply(x_in_new); + accel_report->y = _accel_filter_y.apply(y_in_new); + accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; -- cgit v1.2.3 From c84cdf2ff627fd84c901c869a84d17751323eb97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 16:03:50 +0200 Subject: Enabled filter in makefile --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 255093e67..f1cfc45e6 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,6 +94,7 @@ MODULES += modules/sdlog2 MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 75573e2c2..ae61802c9 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -88,6 +88,7 @@ MODULES += modules/sdlog MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB -- cgit v1.2.3 From 83189a85dacbd56c3aba6ef704d9dd25a85d33da Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Aug 2013 20:25:26 +1000 Subject: l3gd20: disable the FIFO the FIFO was not gaining us anything, and was adding latency. If we use the FIFO we'd need to do multiple SPI transfers to ensure it is drained --- src/drivers/l3gd20/l3gd20.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 9b4bda0ae..ce381807f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -342,7 +342,11 @@ L3GD20::init() write_reg(ADDR_CTRL_REG5, 0); write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_range(2000); /* default to 2000dps */ set_samplerate(0); /* max sample rate */ -- cgit v1.2.3 From 17da1e3f363b0ca79250a2f34588558ceb0146c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:19:48 +0200 Subject: Fixed MS5611 startup on V1 boards --- src/drivers/ms5611/ms5611_i2c.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 06867cc9d..87d9b94a6 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -54,6 +54,8 @@ #include "ms5611.h" +#include "board_config.h" + #ifdef PX4_I2C_OBDEV_MS5611 #ifndef PX4_I2C_BUS_ONBOARD -- cgit v1.2.3 From c2ee4437e0fd362ed8d73203394d34802e9eb48d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:20:55 +0200 Subject: Rate limit sensors, as the in-sensor lowpass allows us to operate at 200-250 Hz --- src/modules/sensors/sensors.cpp | 35 ++++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9185891b..0f1782fca 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -758,12 +758,27 @@ Sensors::accel_init() errx(1, "FATAL: no accelerometer found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the accel internal sampling rate up to at leat 500Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the accel internal sampling rate up to at leat 800Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system accel"); close(fd); } @@ -781,12 +796,27 @@ Sensors::gyro_init() errx(1, "FATAL: no gyro found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the gyro internal sampling rate up to at leat 500Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the gyro internal sampling rate up to at leat 800Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system gyro"); close(fd); } @@ -1387,6 +1417,9 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + /* * do advertisements */ @@ -1422,7 +1455,7 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ -- cgit v1.2.3 From 8b4413f3b1309deff41ff98c457d2e9abe7817d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:09:10 +0200 Subject: Hotfix for attitude estimator EKF init --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 1eff60e88..9e533ccdf 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -236,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - int loopcounter = 0; int printcounter = 0; @@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { + if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); @@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 12000) + printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; -- cgit v1.2.3 From b911d37975244b9e7a5825d8fbff6cc913dc3050 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 4 Aug 2013 19:37:08 -0700 Subject: Merge a path definition from the mainline. --- makefiles/setup.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index fdb161201..168e41a5c 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -45,6 +45,7 @@ export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ +export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ -- cgit v1.2.3 From 58a4c5d5449eeffa0c296cbf5c13058d7412e76b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 5 Aug 2013 16:32:39 +0200 Subject: Added missing include for MS5611 --- src/drivers/ms5611/ms5611_spi.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 1ea698922..f6c624340 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -53,6 +53,7 @@ #include #include "ms5611.h" +#include "board_config.h" /* SPI protocol address bits */ #define DIR_READ (1<<7) -- cgit v1.2.3 From ca9a11a586e8c4994e8be28e1b0e23c3f0d341ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Aug 2013 23:09:02 +0200 Subject: Indendation fixes --- src/drivers/l3gd20/l3gd20.cpp | 6 +++--- src/drivers/lsm303d/lsm303d.cpp | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ce381807f..fde201351 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -189,9 +189,9 @@ private: perf_counter_t _sample_perf; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; /** * Start automatic measurement. diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 56400c843..a95b62468 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -233,9 +233,9 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; /** * Start automatic measurement. @@ -407,10 +407,10 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_range_ga(0.0f), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(800, 30), - _accel_filter_y(800, 30), - _accel_filter_z(800, 30) + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _accel_filter_x(800, 30), + _accel_filter_y(800, 30), + _accel_filter_z(800, 30) { // enable debug() calls _debug_enabled = true; -- cgit v1.2.3 From ec2e02d50ea2f518051b50e4e83e12736526fbc2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 5 Aug 2013 21:05:53 -0700 Subject: Fix CAN2 pinout selection thanks to heads-up from Joe van Niekerk --- nuttx-configs/px4fmu-v1/include/board.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index 1921f7715..27ace4b7d 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -216,8 +216,8 @@ * CAN2 is routed to the expansion connector. */ -#define GPIO_CAN2_RX GPIO_CAN2_RX_2 -#define GPIO_CAN2_TX GPIO_CAN2_TX_2 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* * I2C -- cgit v1.2.3 From 46fd904b5a6002f2fea0495db88c44f3c6a6ee38 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Aug 2013 10:30:32 +1000 Subject: px4io: include board_config.h without this we don't get the I2C interface built for PX4IO --- src/drivers/px4io/px4io_i2c.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index e0c3e5608..19776c40a 100644 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -49,6 +49,7 @@ #include #include +#include #include @@ -165,4 +166,4 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count) return ret; } -#endif /* PX4_I2C_OBDEV_PX4IO */ \ No newline at end of file +#endif /* PX4_I2C_OBDEV_PX4IO */ -- cgit v1.2.3 From 40c56ab61e04fe73aff3a84d20ffc81e102373f3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 6 Aug 2013 21:10:41 +0200 Subject: Corrected bug in px4io driver that lead to hang of FMU-IO communication --- src/drivers/px4io/px4io.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 827b0bb00..bc3f1dcc6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1008,8 +1008,6 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(safety), _to_safety, &safety); - if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; safety.safety_switch_available = true; -- cgit v1.2.3 From 32439d748ad169f6f9956fb3248535730e0374a4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 7 Aug 2013 20:21:49 +0200 Subject: commander: set mode using base_mode and custom_mode --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++++++++++----------- 1 file changed, 41 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2012abcc0..7ede3e1e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -126,10 +126,6 @@ extern struct system_load_s system_load; #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 -// TODO include mavlink headers -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -141,7 +137,13 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -#endif + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; /* Mavlink file descriptors */ static int mavlink_fd; @@ -277,8 +279,9 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (int) cmd->param1; - uint8_t custom_mode = (int) cmd->param2; + uint8_t base_mode = (uint8_t) cmd->param1; + uint32_t custom_mode = (uint32_t) cmd->param2; + // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); /* set arming state */ @@ -286,6 +289,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); } @@ -294,9 +298,11 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); } + } else { arming_res = TRANSITION_NOT_CHANGED; } @@ -305,21 +311,37 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + + } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - if (custom_mode & 1) { // TODO add custom mode flags definition - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else { + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); } @@ -1534,6 +1556,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } + break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1545,6 +1568,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } + break; case LOW_PRIO_TASK_GYRO_CALIBRATION: -- cgit v1.2.3 From 687273ae6fe65fd006492844509a6cd1e25115b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 6 Aug 2013 10:30:32 +1000 Subject: Enable the dedicated interrupt stack for both v1 and v2 boards. This will help save us from threads that are under-provisioned stack-wise. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 129d92149..bcf456c56 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -328,7 +328,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=196608 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=0 +CONFIG_ARCH_INTERRUPTSTACK=2048 # # Boot options diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2e73a5a07..ebbbd8f46 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -380,7 +380,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=0 +CONFIG_ARCH_INTERRUPTSTACK=2048 # # Boot options -- cgit v1.2.3 From a0235bd5071bc8488c585588241422128b1d3361 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Aug 2013 08:36:18 +0200 Subject: Increased buffer sizes for telemetry, set USB PID correctly according to new scheme --- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ebbbd8f46..6e8e239bc 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -664,10 +664,10 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" # CONFIG_USBMSC is not set -- cgit v1.2.3 From 91e54aa5be9930441c85c0585494f8ac8f514681 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Aug 2013 15:51:00 +1000 Subject: add .gdbinit as Debug/dot.gdbinit very useful for JTAG debugging --- Debug/dot.gdbinit | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 Debug/dot.gdbinit diff --git a/Debug/dot.gdbinit b/Debug/dot.gdbinit new file mode 100644 index 000000000..d70410bc7 --- /dev/null +++ b/Debug/dot.gdbinit @@ -0,0 +1,13 @@ +# copy the file to .gdbinit in your Firmware tree, and adjust the path +# below to match your system +# For example: +# target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE5A1C4-if00 +# target extended /dev/ttyACM4 + + +monitor swdp_scan +attach 1 +monitor vector_catch disable hard +set mem inaccessible-by-default off +set print pretty +source Debug/PX4 -- cgit v1.2.3 From 910cf4e5ba20bfa1a18f5bde67c65e83a352f9f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 10 Aug 2013 15:50:26 +1000 Subject: dot.gdbinit should not be ignored --- .gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 5a4f7683c..3c825ca64 100644 --- a/.gitignore +++ b/.gitignore @@ -19,7 +19,6 @@ Archives/* Build/* core cscope.out -dot.gdbinit Firmware.sublime-workspace Images/*.bin Images/*.px4 @@ -30,4 +29,4 @@ nuttx-configs/px4io-v2/src/Make.dep nuttx-configs/px4io-v2/src/libboard.a nuttx-configs/px4io-v1/src/.depend nuttx-configs/px4io-v1/src/Make.dep -nuttx-configs/px4io-v1/src/libboard.a \ No newline at end of file +nuttx-configs/px4io-v1/src/libboard.a -- cgit v1.2.3 From cbb5ce8f59a34615fe0d702041c497efe40edb56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 16:54:00 +0200 Subject: Fixed startup behavior for PX4 autostart --- ROMFS/px4fmu_common/init.d/10_io_f330 | 10 ++++------ ROMFS/px4fmu_common/init.d/rc.sensors | 3 --- ROMFS/px4fmu_common/init.d/rc.usb | 14 ++++++++++++++ src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- 5 files changed, 20 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 4450eb50d..4083bb905 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -75,6 +75,8 @@ px4io min 1200 1200 1200 1200 # Upper limits could be higher, this is on the safe side # px4io max 1800 1800 1800 1800 + +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Start the sensors (depends on orb, px4io) @@ -95,17 +97,13 @@ gps start # Start the attitude estimator (depends on orb) # attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + multirotor_att_control start # # Start logging # -sdlog2 start -r 20 -a -b 16 +sdlog2 start -r 50 -a -b 16 # # Start system state diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5e80ddc2f..26b561571 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -36,8 +36,5 @@ fi # if sensors start then - # - # Check sensors - run AFTER 'sensors start' - # preflight_check & fi diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 5b1bd272e..abeed0ca3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -16,12 +16,26 @@ fi sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 +# Stop commander +if commander stop +then + echo "Commander stopped" +fi +sleep 1 + # Start the commander if commander start then echo "Commander started" fi +# Stop px4io +if px4io stop +then + echo "PX4IO stopped" +fi +sleep 1 + # Start px4io if present if px4io start then diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index fde201351..42a0c264c 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -780,7 +780,7 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a95b62468..117583faf 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1370,7 +1370,7 @@ start() int fd, fd_mag; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); -- cgit v1.2.3 From 42b496178136a398447742f0efc81348845087e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 16:54:24 +0200 Subject: Reduced excessive IO stack size (had 4k, is using 0.7k, has now 2k) --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 281debe5c..1122195d4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -558,7 +558,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); -- cgit v1.2.3 From 66d294b5bf3972c1f49ea452964f32e18b25b2d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 17:39:10 +0200 Subject: Fixed to FMUv2 autostart and config --- ROMFS/px4fmu_common/init.d/10_io_f330 | 30 ++++++++++++++++-------------- ROMFS/px4fmu_common/init.d/rc.sensors | 2 ++ makefiles/config_px4fmu-v2_default.mk | 2 +- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 4083bb905..13272426e 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -56,11 +56,6 @@ pwm -u 400 -m 0xff # this is very unlikely, but quite safe and robust. px4io recovery -# -# Disable px4io topic limiting -# -px4io limit 200 - # # This sets a PWM right after startup (regardless of safety button) # @@ -99,16 +94,23 @@ gps start attitude_estimator_ekf start multirotor_att_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 16 - + # -# Start system state +# Disable px4io topic limiting and start logging # -if blinkm start +if [ $BOARD == fmuv1 ] then - blinkm systemstate + px4io limit 200 + sdlog2 start -r 50 -a -b 16 + if blinkm start + then + blinkm systemstate + fi +else + px4io limit 400 + sdlog2 start -r 100 -a -b 16 + if rgbled start + then + #rgbled systemstate + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 26b561571..17591be5b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -22,10 +22,12 @@ if mpu6000 start then echo "using MPU6000 and HMC5883L" hmc5883 start + set BOARD fmuv1 else echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start + set BOARD fmuv2 fi # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ae61802c9..cc182e6af 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -80,7 +80,7 @@ MODULES += modules/multirotor_pos_control # # Logging # -MODULES += modules/sdlog +MODULES += modules/sdlog2 # # Library modules -- cgit v1.2.3 From 083cc60acb8b602864d0727e09218eeeca5eb980 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Aug 2013 18:42:20 +0200 Subject: Increased logging to 200 Hz in F330 startup for v2, allowed to set up to 333 Hz update rate in IO driver for v2 link --- ROMFS/px4fmu_common/init.d/10_io_f330 | 2 +- src/drivers/px4io/px4io.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 13272426e..b3fb02757 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -108,7 +108,7 @@ then fi else px4io limit 400 - sdlog2 start -r 100 -a -b 16 + sdlog2 start -r 200 -a -b 16 if rgbled start then #rgbled systemstate diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1122195d4..ae5d6ce19 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1628,8 +1628,8 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; - if (interval_ms < 5) { - interval_ms = 5; + if (interval_ms < 3) { + interval_ms = 3; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); } @@ -1956,7 +1956,7 @@ px4io_main(int argc, char *argv[]) if ((argc > 2)) { g_dev->set_update_rate(atoi(argv[2])); } else { - errx(1, "missing argument (50 - 200 Hz)"); + errx(1, "missing argument (50 - 400 Hz)"); return 1; } exit(0); -- cgit v1.2.3 From f36a2ff45a9f24637ea360cdadd5f168c5f50946 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 10 Aug 2013 12:39:58 -0700 Subject: Add a 'menuconfig' target that makes it possible to use the NuttX menuconfig tool on the PX4 config files. --- Makefile | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 82a5ffd27..9ef30c2df 100644 --- a/Makefile +++ b/Makefile @@ -148,7 +148,7 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)/nuttx-configs/$(board) .) + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export @@ -156,6 +156,32 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) +# +# The user can run the NuttX 'menuconfig' tool for a single board configuration with +# make BOARDS= menuconfig +# +ifeq ($(MAKECMDGOALS),menuconfig) +ifneq ($(words $(BOARDS)),1) +$(error BOARDS must specify exactly one board for the menuconfig goal) +endif +BOARD = $(BOARDS) +menuconfig: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(BOARD) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) + @$(ECHO) %% Running menuconfig for $(BOARD) + $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + @$(ECHO) %% Saving configuration file + $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig +else +menuconfig: + @$(ECHO) "" + @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" + @$(ECHO) "" +endif + $(NUTTX_SRC): @$(ECHO) "" @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." -- cgit v1.2.3 From 70f272bd22e9ccdb9dbc1c15dd76fce4449ea0ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 13:44:11 +0200 Subject: Disabled SDIO DMA, enabled CCM memory --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 6e8e239bc..da40d18b0 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -95,7 +95,7 @@ CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_DISABLE_REORDERING=y -CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMA=n # CONFIG_SDIO_DMAPRIO is not set # CONFIG_SDIO_WIDTH_D1_ONLY is not set @@ -257,7 +257,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -CONFIG_STM32_CCMEXCLUDE=y +CONFIG_STM32_CCMEXCLUDE=n CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set -- cgit v1.2.3 From e1037e20bea85834d7080a08b5a99d3dec5c0d41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 13:57:33 +0200 Subject: Fixed inconsistend defconfig - switching to menuconfig ASAP --- nuttx-configs/px4fmu-v2/nsh/defconfig | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index da40d18b0..886fdcb96 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -95,9 +95,6 @@ CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_DISABLE_REORDERING=y -CONFIG_SDIO_DMA=n -# CONFIG_SDIO_DMAPRIO is not set -# CONFIG_SDIO_WIDTH_D1_ONLY is not set # # STM32 Configuration Options @@ -339,7 +336,9 @@ CONFIG_MTD=y # # STM32 SDIO-based MMC/SD driver # -CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMA=n +# CONFIG_SDIO_DMAPRIO is not set +# CONFIG_SDIO_WIDTH_D1_ONLY is not set CONFIG_MMCSD_MULTIBLOCK_DISABLE=y CONFIG_MMCSD_MMCSUPPORT=n CONFIG_MMCSD_HAVECARDDETECT=n -- cgit v1.2.3 From 92fc6a05c3c5fb4b498a300b0d3190d7c2b14926 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 14:07:42 +0200 Subject: Putting SDIO back to DMA and disabling CCM again --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 886fdcb96..9454d116a 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -254,7 +254,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -CONFIG_STM32_CCMEXCLUDE=n +CONFIG_STM32_CCMEXCLUDE=y CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set @@ -336,7 +336,7 @@ CONFIG_MTD=y # # STM32 SDIO-based MMC/SD driver # -CONFIG_SDIO_DMA=n +CONFIG_SDIO_DMA=y # CONFIG_SDIO_DMAPRIO is not set # CONFIG_SDIO_WIDTH_D1_ONLY is not set CONFIG_MMCSD_MULTIBLOCK_DISABLE=y -- cgit v1.2.3 From d7730a3444b1c4277bca24988402839a98a52fdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 10:59:22 +0200 Subject: commander, mavlink: fixed base_mode and custom_mode in mavlink --- src/modules/commander/commander.cpp | 14 ++++------ src/modules/mavlink/mavlink.c | 52 ++++++++++++++++++------------------- src/modules/mavlink/orb_listener.c | 22 +++++++++------- src/modules/mavlink/util.h | 2 +- 4 files changed, 43 insertions(+), 47 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ede3e1e6..82b575405 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include +#include "px4_custom_mode.h" #include "commander_helper.h" #include "state_machine_helper.h" #include "calibration_routines.h" @@ -138,13 +139,6 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, -}; - /* Mavlink file descriptors */ static int mavlink_fd; @@ -1321,8 +1315,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + if (leds_counter & 8) { + led_on(LED_AMBER); + } else { + led_off(LED_AMBER); } } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9132d1b49..6d9ca1120 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,6 +64,7 @@ #include #include #include +#include #include "waypoints.h" #include "orb_topics.h" @@ -181,10 +182,11 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; /** * Set mode flags @@ -192,36 +194,31 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* HIL */ if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* autonomous mode */ - if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; - } - - /* set arming state */ + /* arming state */ if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.main_state == MAIN_STATE_MANUAL - || v_status.main_state == MAIN_STATE_SEATBELT - || v_status.main_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; } - if (v_status.navigation_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /** * Set mavlink state **/ @@ -645,11 +642,12 @@ int mavlink_thread_main(int argc, char *argv[]) /* 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(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ if (v_status.hil_state == HIL_STATE_ON) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d088d421e..2a260861d 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -279,15 +279,16 @@ l_vehicle_status(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.navigation_state, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -473,8 +474,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -491,7 +493,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -505,7 +507,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -519,7 +521,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -533,7 +535,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index a4ff06a88..5e5ee8261 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); -- cgit v1.2.3 From d0a9d250f74f9f1386760af3d324480d173a1b43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Aug 2013 14:57:30 +0200 Subject: Enforced consistency between configs --- nuttx-configs/px4fmu-v2/nsh/defconfig | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9454d116a..85bf6dd2f 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -640,6 +640,7 @@ CONFIG_USBDEV=y # CONFIG_USBDEV_SELFPOWERED is not set CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set -- cgit v1.2.3 From 21a919d973c13d7d2259b47116480ade819d7b8c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Aug 2013 14:49:32 +1000 Subject: rgbled: added LED_MODE_RGB to allow for constant RGB values this allows apps to fully control the 3 LED elements --- src/drivers/rgbled/rgbled.cpp | 48 +++++++++++++++++++++++++++++++++++++------ src/include/device/rgbled.h | 14 +++++++++++++ 2 files changed, 56 insertions(+), 6 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 5c4fa4bb6..427537508 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -80,7 +80,8 @@ enum ledModes { LED_MODE_TEST, LED_MODE_SYSTEMSTATE, - LED_MODE_OFF + LED_MODE_OFF, + LED_MODE_RGB }; class RGBLED : public device::I2C @@ -119,6 +120,9 @@ private: int led_colors[8]; int led_blink; + // RGB values for MODE_RGB + struct RGBLEDSet rgb; + int mode; int running; @@ -178,6 +182,7 @@ RGBLED::setMode(enum ledModes new_mode) switch (new_mode) { case LED_MODE_SYSTEMSTATE: case LED_MODE_TEST: + case LED_MODE_RGB: mode = new_mode; if (!running) { running = true; @@ -185,6 +190,7 @@ RGBLED::setMode(enum ledModes new_mode) work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } break; + case LED_MODE_OFF: default: @@ -237,6 +243,12 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { + case RGBLED_SET: { + /* set the specified RGB values */ + memcpy(&rgb, (struct RGBLEDSet *)arg, sizeof(rgb)); + setMode(LED_MODE_RGB); + return OK; + } default: break; @@ -290,6 +302,11 @@ RGBLED::led() break; + case LED_MODE_RGB: + set_rgb(rgb.red, rgb.green, rgb.blue); + running = false; + return; + case LED_MODE_OFF: default: return; @@ -308,10 +325,12 @@ RGBLED::led() led_thread_runcount++; - if(running) { /* re-queue ourselves to run again later */ work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); + } else if (mode == LED_MODE_RGB) { + // no need to run again until the colour changes + set_on(true); } else { set_on(false); } @@ -412,7 +431,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off'"); + warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off', 'rgb'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -461,9 +480,9 @@ rgbled_main(int argc, char *argv[]) /* need the driver past this point */ if (g_rgbled == nullptr) { - fprintf(stderr, "not started\n"); - rgbled_usage(); - exit(0); + fprintf(stderr, "not started\n"); + rgbled_usage(); + exit(0); } if (!strcmp(verb, "test")) { @@ -486,5 +505,22 @@ rgbled_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "rgb")) { + int fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + if (argc < 4) { + errx(1, "Usage: rgbled rgb "); + } + struct RGBLEDSet v; + v.red = strtol(argv[1], NULL, 0); + v.green = strtol(argv[2], NULL, 0); + v.blue = strtol(argv[3], NULL, 0); + int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); + close(fd); + exit(ret); + } + rgbled_usage(); } diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h index a18920892..600a65d28 100644 --- a/src/include/device/rgbled.h +++ b/src/include/device/rgbled.h @@ -65,3 +65,17 @@ * The script is terminated by a zero command. */ #define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) + +/** set constant RGB values */ +#define RGBLED_SET _RGBLEDIOC(4) + +/* + structure passed to RGBLED_SET ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +struct RGBLEDSet { + uint8_t red; + uint8_t green; + uint8_t blue; +}; -- cgit v1.2.3 From 3b10f8431def73222823c1c2abe1bb7422d851dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Aug 2013 14:59:58 +1000 Subject: rgbled: try expansion bus first, unless specific bus given this will allow "rgbled start" to detect which bus the LED is on, supporting boards with either external or internal LEDs --- src/drivers/rgbled/rgbled.cpp | 35 ++++++++++++++++++++++++----------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 427537508..44aa922e6 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -166,7 +166,6 @@ RGBLED::init() ret = I2C::init(); if (ret != OK) { - warnx("I2C init failed"); return ret; } @@ -440,7 +439,7 @@ void rgbled_usage() { int rgbled_main(int argc, char *argv[]) { - int i2cdevice = PX4_I2C_BUS_LED; + int i2cdevice = -1; int rgbledadr = ADDR; /* 7bit */ int ch; @@ -464,15 +463,29 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled != nullptr) errx(1, "already started"); - g_rgbled = new RGBLED(i2cdevice, rgbledadr); - - if (g_rgbled == nullptr) - errx(1, "new failed"); - - if (OK != g_rgbled->init()) { - delete g_rgbled; - g_rgbled = nullptr; - errx(1, "init failed"); + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + if (g_rgbled == nullptr) { + // fall back to default bus + i2cdevice = PX4_I2C_BUS_LED; + } + } + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + if (g_rgbled == nullptr) + errx(1, "new failed"); + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } } exit(0); -- cgit v1.2.3 From 29d78367846ebf7834ecd87b2cf528573c3fcdd8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 10:53:47 +0200 Subject: RGBled fixes: options and off after rgb working now --- src/drivers/rgbled/rgbled.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 44aa922e6..236f138a7 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -195,8 +195,8 @@ RGBLED::setMode(enum ledModes new_mode) default: if (running) { running = false; - set_on(false); } + set_on(false); mode = LED_MODE_OFF; break; } @@ -443,7 +443,8 @@ rgbled_main(int argc, char *argv[]) int rgbledadr = ADDR; /* 7bit */ int ch; - while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -455,9 +456,8 @@ rgbled_main(int argc, char *argv[]) rgbled_usage(); } } - argc -= optind; - argv += optind; - const char *verb = argv[0]; + + const char *verb = argv[1]; if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) @@ -523,13 +523,13 @@ rgbled_main(int argc, char *argv[]) if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - if (argc < 4) { + if (argc < 5) { errx(1, "Usage: rgbled rgb "); } struct RGBLEDSet v; - v.red = strtol(argv[1], NULL, 0); - v.green = strtol(argv[2], NULL, 0); - v.blue = strtol(argv[3], NULL, 0); + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); close(fd); exit(ret); -- cgit v1.2.3 From 9505654f9103c8965891991514ea690b3e6aea25 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 17:57:01 +0200 Subject: commander/px4_custom_mode.h added --- src/modules/commander/px4_custom_mode.h | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 src/modules/commander/px4_custom_mode.h diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h new file mode 100644 index 000000000..4d4859a5c --- /dev/null +++ b/src/modules/commander/px4_custom_mode.h @@ -0,0 +1,18 @@ +/* + * px4_custom_mode.h + * + * Created on: 09.08.2013 + * Author: ton + */ + +#ifndef PX4_CUSTOM_MODE_H_ +#define PX4_CUSTOM_MODE_H_ + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; + +#endif /* PX4_CUSTOM_MODE_H_ */ -- cgit v1.2.3 From 53def47216d5bbacdfdb76428c024ba3feaea64e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Aug 2013 17:23:51 +0200 Subject: Fixed gyro scale calibration (it was storing scale even though the scale calibration was skipped --- src/modules/commander/gyro_calibration.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 9e6909db0..60b747ee0 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -177,8 +177,12 @@ void do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) - break; + && (hrt_absolute_time() - start_time > 5 * 1e6)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + tune_positive(); + return; + } /* wait blocking for new data */ struct pollfd fds[1]; -- cgit v1.2.3 From 27d0885453711a3d3ab6abf3cf227afc837e14bd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 22:38:14 +0200 Subject: gyro_calibration: confusing typo in mavlink message fixed --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 60b747ee0..f1afb0107 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -154,7 +154,7 @@ void do_gyro_calibration(int mavlink_fd) /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); -- cgit v1.2.3 From 50cf1c01b701fced6437dfe574fd09cd312b9f15 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 14 Aug 2013 22:29:36 -0700 Subject: Compile fix. --- src/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 83a1a1abb..e79d7e10a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -59,7 +59,7 @@ #include #include -#include +#include #include #include "chip.h" -- cgit v1.2.3 From 0bbc4b7012c72fda61dca01a897552e9483a4f5f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 08:42:08 +0200 Subject: Build fixes --- makefiles/config_px4fmu-v1_default.mk | 2 +- src/drivers/px4io/px4io.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 2f70d001d..33ffba6bf 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -76,7 +76,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX needs state machine update MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2953639bf..65e8fa4b6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -870,7 +870,7 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - _system_armed = vstatus.flag_system_armed; + _system_armed = armed.armed; if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; -- cgit v1.2.3 From cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 09:52:22 +0200 Subject: Rejecting arming if safety switch is not in safe position, added reboot command --- src/modules/commander/commander.cpp | 49 ++++++++++++++++++-------- src/modules/commander/state_machine_helper.cpp | 21 +++++++++-- src/modules/commander/state_machine_helper.h | 6 +++- 3 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b575405..d4c2c4c84 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -265,7 +265,7 @@ void usage(const char *reason) exit(1); } -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -282,7 +282,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -291,7 +291,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -356,6 +356,23 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(status, safety, armed)) { + + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -388,7 +405,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* check if we have new task and no other task is scheduled */ if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { + if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; low_prio_task = new_low_prio_task; @@ -407,10 +424,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ @@ -605,6 +622,8 @@ int commander_thread_main(int argc, char *argv[]) int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; memset(&safety, 0, sizeof(safety)); + safety.safety_switch_available = false; + safety.safety_off = false; /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -746,7 +765,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &control_mode, &cmd, &armed); + handle_command(&status, &safety, &control_mode, &cmd, &armed); } /* update safety topic */ @@ -871,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); status_changed = true; } @@ -887,7 +906,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1026,7 +1045,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1045,7 +1064,7 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); stick_on_counter = 0; } else { @@ -1168,10 +1187,10 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } } else { @@ -1243,7 +1262,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + if (!arm_tune_played && armed.armed) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1540,6 +1559,8 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); + low_prio_task = LOW_PRIO_TASK_NONE; + while (!thread_should_exit) { switch (low_prio_task) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06cd060c5..163aceed2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -108,8 +108,10 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ + { ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -172,6 +174,19 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi return ret; } +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + } else { + return false; + } +} + bool check_arming_state_changed() { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index c8c77e5d8..a38c2497e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include #include typedef enum { @@ -56,7 +57,10 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed); + +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); bool check_arming_state_changed(); -- cgit v1.2.3 From e5af29be1706b1d20d6bafe8c481213c76cc0d34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 12:38:25 +0200 Subject: Fixed param save and load --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b575405..5b3654bcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -407,10 +407,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ -- cgit v1.2.3 From 39ae01dd07d53e3509826ae3737fc6a509adec34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 13:29:42 +0200 Subject: Fix the low priority loop, calibration routines work again --- src/modules/commander/commander.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5b3654bcd..c6e209f0f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1552,7 +1552,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1564,37 +1564,43 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1604,8 +1610,6 @@ void *commander_low_prio_loop(void *arg) break; } - low_prio_task = LOW_PRIO_TASK_NONE; - } return 0; -- cgit v1.2.3 From f51320d1afac836085ee4d9dbdaeda7af18bcbda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 41e22d21d..71f33d81b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ -- cgit v1.2.3 From 98f403002f72e7fb3e38398de9d87746f7918347 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5de99040c..1ae88d17a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ -- cgit v1.2.3 From 0c4e3dce0ef82ea3a7dd2b7bed8b23108f34205d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:34:49 +0200 Subject: Added LED_TOGGLE for normal LEDs --- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 19 +++++++++++++++++++ src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 12 ++++++++++++ src/drivers/drv_led.h | 1 + src/drivers/led/led.cpp | 6 ++++++ 4 files changed, 38 insertions(+) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index aa83ec130..ea91f34ad 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -57,6 +57,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS __EXPORT void led_init() @@ -94,3 +95,21 @@ __EXPORT void led_off(int led) stm32_gpiowrite(GPIO_LED2, true); } } + +__EXPORT void led_toggle(int led) +{ + if (led == 0) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } + if (led == 1) + { + if (stm32_gpioread(GPIO_LED2)) + stm32_gpiowrite(GPIO_LED2, false); + else + stm32_gpiowrite(GPIO_LED2, true); + } +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 5ded4294e..a856ccb02 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -57,6 +57,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS __EXPORT void led_init() @@ -83,3 +84,14 @@ __EXPORT void led_off(int led) stm32_gpiowrite(GPIO_LED1, true); } } + +__EXPORT void led_toggle(int led) +{ + if (led == 1) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 97f2db107..4ce04696e 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -54,6 +54,7 @@ #define LED_ON _IOC(_LED_BASE, 0) #define LED_OFF _IOC(_LED_BASE, 1) +#define LED_TOGGLE _IOC(_LED_BASE, 2) __BEGIN_DECLS diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index fe307223f..a37eaca53 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -52,6 +52,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS class LED : device::CDev @@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg) led_off(arg); break; + case LED_TOGGLE: + led_toggle(arg); + break; + + default: result = CDev::ioctl(filp, cmd, arg); } -- cgit v1.2.3 From 901cef922cb286a247872bd2ea46ec13f779d61e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:43:01 +0200 Subject: Some cleanup for the RGB LED driver, added ioctl to set color --- src/drivers/drv_rgbled.h | 103 +++++++++++++++++++++++++ src/drivers/rgbled/rgbled.cpp | 171 +++++++++++++++++++----------------------- src/include/device/rgbled.h | 81 -------------------- 3 files changed, 182 insertions(+), 173 deletions(-) create mode 100644 src/drivers/drv_rgbled.h delete mode 100644 src/include/device/rgbled.h diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h new file mode 100644 index 000000000..f7cc5809a --- /dev/null +++ b/src/drivers/drv_rgbled.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 drv_rgbled.h + * + * RGB led device API + */ + +#pragma once + +#include +#include + +/* more devices will be 1, 2, etc */ +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +/* + * ioctl() definitions + */ + +#define _RGBLEDIOCBASE (0x2900) +#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) + +/** play the named script in *(char *)arg, repeating forever */ +#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) + +/** play the numbered script in (arg), repeating forever */ +#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) + +/** + * Set the user script; (arg) is a pointer to an array of script lines, + * where each line is an array of four bytes giving , , arg[0-2] + * + * The script is terminated by a zero command. + */ +#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) + +/** set constant RGB values */ +#define RGBLED_SET _RGBLEDIOC(4) + +/** set color */ +#define RGBLED_SET_COLOR _RGBLEDIOC(5) + +/* + structure passed to RGBLED_SET ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +struct RGBLEDSet { + uint8_t red; + uint8_t green; + uint8_t blue; +}; + +typedef enum { + RGBLED_COLOR_OFF, + RGBLED_COLOR_RED, + RGBLED_COLOR_YELLOW, + RGBLED_COLOR_PURPLE, + RGBLED_COLOR_GREEN, + RGBLED_COLOR_BLUE, + RGBLED_COLOR_WHITE, + RGBLED_COLOR_AMBER, +} rgbled_color_t; + +typedef enum { + RGBLED_BLINK_ON, + RGBLED_BLINK_FAST, + RGBLED_BLINK_NORMAL, + RGBLED_BLINK_SLOW, + RGBLED_BLINK_OFF +} rgbled_blinkmode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 44aa922e6..35fdc5158 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -61,10 +61,10 @@ #include -#include "device/rgbled.h" +#include -#define LED_ONTIME 120 -#define LED_OFFTIME 120 +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 #define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ @@ -78,10 +78,10 @@ enum ledModes { - LED_MODE_TEST, - LED_MODE_SYSTEMSTATE, - LED_MODE_OFF, - LED_MODE_RGB + RGBLED_MODE_TEST, + RGBLED_MODE_SYSTEMSTATE, + RGBLED_MODE_OFF, + RGBLED_MODE_RGB }; class RGBLED : public device::I2C @@ -98,35 +98,18 @@ public: virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: - - enum ledColors { - LED_COLOR_OFF, - LED_COLOR_RED, - LED_COLOR_YELLOW, - LED_COLOR_PURPLE, - LED_COLOR_GREEN, - LED_COLOR_BLUE, - LED_COLOR_WHITE, - LED_COLOR_AMBER, - }; - - enum ledBlinkModes { - LED_BLINK_ON, - LED_BLINK_OFF - }; - work_s _work; - int led_colors[8]; - int led_blink; + rgbled_color_t _led_colors[8]; + rgbled_blinkmode_t _led_blinkmode; // RGB values for MODE_RGB - struct RGBLEDSet rgb; + struct RGBLEDSet _rgb; - int mode; - int running; + int _mode; + int _running; - void setLEDColor(int ledcolor); + void setLEDColor(rgbled_color_t ledcolor); static void led_trampoline(void *arg); void led(); @@ -147,10 +130,10 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - led_colors({0,0,0,0,0,0,0,0}), - led_blink(LED_BLINK_OFF), - mode(LED_MODE_OFF), - running(false) + _led_colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), + _led_blinkmode(RGBLED_BLINK_OFF), + _mode(RGBLED_MODE_OFF), + _running(false) { memset(&_work, 0, sizeof(_work)); } @@ -179,25 +162,25 @@ int RGBLED::setMode(enum ledModes new_mode) { switch (new_mode) { - case LED_MODE_SYSTEMSTATE: - case LED_MODE_TEST: - case LED_MODE_RGB: - mode = new_mode; - if (!running) { - running = true; + case RGBLED_MODE_SYSTEMSTATE: + case RGBLED_MODE_TEST: + case RGBLED_MODE_RGB: + _mode = new_mode; + if (!_running) { + _running = true; set_on(true); work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); } break; - case LED_MODE_OFF: + case RGBLED_MODE_OFF: default: - if (running) { - running = false; + if (_running) { + _running = false; set_on(false); } - mode = LED_MODE_OFF; + _mode = RGBLED_MODE_OFF; break; } @@ -244,10 +227,14 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case RGBLED_SET: { /* set the specified RGB values */ - memcpy(&rgb, (struct RGBLEDSet *)arg, sizeof(rgb)); - setMode(LED_MODE_RGB); + memcpy(&_rgb, (struct RGBLEDSet *)arg, sizeof(_rgb)); + setMode(RGBLED_MODE_RGB); return OK; } + case RGBLED_SET_COLOR: { + /* set the specified color name */ + setLEDColor((rgbled_color_t)arg); + } default: break; @@ -271,42 +258,42 @@ void RGBLED::led() { static int led_thread_runcount=0; - static int led_interval = 1000; + static int _led_interval = 1000; - switch (mode) { - case LED_MODE_TEST: + switch (_mode) { + case RGBLED_MODE_TEST: /* Demo LED pattern for now */ - led_colors[0] = LED_COLOR_YELLOW; - led_colors[1] = LED_COLOR_AMBER; - led_colors[2] = LED_COLOR_RED; - led_colors[3] = LED_COLOR_PURPLE; - led_colors[4] = LED_COLOR_BLUE; - led_colors[5] = LED_COLOR_GREEN; - led_colors[6] = LED_COLOR_WHITE; - led_colors[7] = LED_COLOR_OFF; - led_blink = LED_BLINK_ON; + _led_colors[0] = RGBLED_COLOR_YELLOW; + _led_colors[1] = RGBLED_COLOR_AMBER; + _led_colors[2] = RGBLED_COLOR_RED; + _led_colors[3] = RGBLED_COLOR_PURPLE; + _led_colors[4] = RGBLED_COLOR_BLUE; + _led_colors[5] = RGBLED_COLOR_GREEN; + _led_colors[6] = RGBLED_COLOR_WHITE; + _led_colors[7] = RGBLED_COLOR_OFF; + _led_blinkmode = RGBLED_BLINK_ON; break; - case LED_MODE_SYSTEMSTATE: + case RGBLED_MODE_SYSTEMSTATE: /* XXX TODO set pattern */ - led_colors[0] = LED_COLOR_OFF; - led_colors[1] = LED_COLOR_OFF; - led_colors[2] = LED_COLOR_OFF; - led_colors[3] = LED_COLOR_OFF; - led_colors[4] = LED_COLOR_OFF; - led_colors[5] = LED_COLOR_OFF; - led_colors[6] = LED_COLOR_OFF; - led_colors[7] = LED_COLOR_OFF; - led_blink = LED_BLINK_OFF; + _led_colors[0] = RGBLED_COLOR_OFF; + _led_colors[1] = RGBLED_COLOR_OFF; + _led_colors[2] = RGBLED_COLOR_OFF; + _led_colors[3] = RGBLED_COLOR_OFF; + _led_colors[4] = RGBLED_COLOR_OFF; + _led_colors[5] = RGBLED_COLOR_OFF; + _led_colors[6] = RGBLED_COLOR_OFF; + _led_colors[7] = RGBLED_COLOR_OFF; + _led_blinkmode = RGBLED_BLINK_OFF; break; - case LED_MODE_RGB: - set_rgb(rgb.red, rgb.green, rgb.blue); - running = false; + case RGBLED_MODE_RGB: + set_rgb(_rgb.red, _rgb.green, _rgb.blue); + _running = false; return; - case LED_MODE_OFF: + case RGBLED_MODE_OFF: default: return; break; @@ -314,20 +301,20 @@ RGBLED::led() if (led_thread_runcount & 1) { - if (led_blink == LED_BLINK_ON) - setLEDColor(LED_COLOR_OFF); - led_interval = LED_OFFTIME; + if (_led_blinkmode == RGBLED_BLINK_ON) + setLEDColor(RGBLED_COLOR_OFF); + _led_interval = RGBLED_OFFTIME; } else { - setLEDColor(led_colors[(led_thread_runcount/2) % 8]); - led_interval = LED_ONTIME; + setLEDColor(_led_colors[(led_thread_runcount/2) % 8]); + _led_interval = RGBLED_ONTIME; } led_thread_runcount++; - if(running) { + if(_running) { /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, led_interval); - } else if (mode == LED_MODE_RGB) { + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); + } else if (_mode == RGBLED_MODE_RGB) { // no need to run again until the colour changes set_on(true); } else { @@ -335,30 +322,30 @@ RGBLED::led() } } -void RGBLED::setLEDColor(int ledcolor) { +void RGBLED::setLEDColor(rgbled_color_t ledcolor) { switch (ledcolor) { - case LED_COLOR_OFF: // off + case RGBLED_COLOR_OFF: // off set_rgb(0,0,0); break; - case LED_COLOR_RED: // red + case RGBLED_COLOR_RED: // red set_rgb(255,0,0); break; - case LED_COLOR_YELLOW: // yellow + case RGBLED_COLOR_YELLOW: // yellow set_rgb(255,70,0); break; - case LED_COLOR_PURPLE: // purple + case RGBLED_COLOR_PURPLE: // purple set_rgb(255,0,255); break; - case LED_COLOR_GREEN: // green + case RGBLED_COLOR_GREEN: // green set_rgb(0,255,0); break; - case LED_COLOR_BLUE: // blue + case RGBLED_COLOR_BLUE: // blue set_rgb(0,0,255); break; - case LED_COLOR_WHITE: // white + case RGBLED_COLOR_WHITE: // white set_rgb(255,255,255); break; - case LED_COLOR_AMBER: // amber + case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; } @@ -499,12 +486,12 @@ rgbled_main(int argc, char *argv[]) } if (!strcmp(verb, "test")) { - g_rgbled->setMode(LED_MODE_TEST); + g_rgbled->setMode(RGBLED_MODE_TEST); exit(0); } if (!strcmp(verb, "systemstate")) { - g_rgbled->setMode(LED_MODE_SYSTEMSTATE); + g_rgbled->setMode(RGBLED_MODE_SYSTEMSTATE); exit(0); } @@ -514,7 +501,7 @@ rgbled_main(int argc, char *argv[]) } if (!strcmp(verb, "off")) { - g_rgbled->setMode(LED_MODE_OFF); + g_rgbled->setMode(RGBLED_MODE_OFF); exit(0); } diff --git a/src/include/device/rgbled.h b/src/include/device/rgbled.h deleted file mode 100644 index 600a65d28..000000000 --- a/src/include/device/rgbled.h +++ /dev/null @@ -1,81 +0,0 @@ -/**************************************************************************** - * - * 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 rgbled.h - * - * RGB led device API - */ - -#pragma once - -#include -#include - -/* more devices will be 1, 2, etc */ -#define RGBLED_DEVICE_PATH "/dev/rgbled0" - -/* - * ioctl() definitions - */ - -#define _RGBLEDIOCBASE (0x2900) -#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) - -/** play the named script in *(char *)arg, repeating forever */ -#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) - -/** play the numbered script in (arg), repeating forever */ -#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) - -/** - * Set the user script; (arg) is a pointer to an array of script lines, - * where each line is an array of four bytes giving , , arg[0-2] - * - * The script is terminated by a zero command. - */ -#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) - -/** set constant RGB values */ -#define RGBLED_SET _RGBLEDIOC(4) - -/* - structure passed to RGBLED_SET ioctl() - Note that the driver scales the brightness to 0 to 255, regardless - of the hardware scaling - */ -struct RGBLEDSet { - uint8_t red; - uint8_t green; - uint8_t blue; -}; -- cgit v1.2.3 From d75c3c4e7369510db1d91721c2793c23dcd873fa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:48:28 +0200 Subject: Try to open RGBLEDs in commander (WIP) --- src/modules/commander/commander.cpp | 9 ++---- src/modules/commander/commander_helper.cpp | 45 +++++++++++++++++++++++------- 2 files changed, 38 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71f33d81b..4e6ecd1e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1331,11 +1331,11 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter & 8) { - led_on(LED_AMBER); + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); } else { - led_off(LED_AMBER); + led_toggle(LED_AMBER); } } else { @@ -1358,9 +1358,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } leds_counter++; - - if (leds_counter >= 16) - leds_counter = 0; } void diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 681a11568..d9b255f4f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -53,6 +53,8 @@ #include #include #include +#include + #include "commander_helper.h" @@ -136,9 +138,11 @@ void tune_stop() } static int leds; +static int rgbleds; int led_init() { + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { @@ -146,10 +150,29 @@ int led_init() return ERROR; } - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); + /* the blue LED is only available on FMUv1 but not FMUv2 */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + if (ioctl(leds, LED_ON, LED_BLUE)) { + warnx("Blue LED: ioctl fail\n"); return ERROR; } +#endif + + if (ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: ioctl fail\n"); + return ERROR; + } + + /* then try RGB LEDs, this can fail on FMUv1*/ + rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "Unable to open " RGBLED_DEVICE_PATH); +#else + warnx("No RGB LED found"); +#endif + } return 0; } @@ -157,18 +180,15 @@ int led_init() void led_deinit() { close(leds); + + if (rgbleds != -1) { + close(rgbleds); + } } int led_toggle(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); + return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) @@ -181,6 +201,11 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } +int rgbled_set_color(rgbled_color_t color) { + + return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color); +} + float battery_remaining_estimate_voltage(float voltage) { float ret = 0; -- cgit v1.2.3 From 3f9f1d60e03f501209deb6c7532c37dfb786f343 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 09:23:39 +0200 Subject: Added audio and text warning if arming is refused due to mode switch --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ae88d17a..6181dafab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -204,6 +204,7 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); +void print_reject_arm(const char *msg); transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); @@ -1082,6 +1083,16 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates safety switch not pressed */ + + if (!(!safety.safety_switch_available || safety.safety_off)) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } } /* fill current_status according to mode switches */ @@ -1490,6 +1501,20 @@ print_reject_mode(const char *msg) } } +void +print_reject_arm(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { -- cgit v1.2.3 From ec9de4ad84be8e62b762567c58ec3bb948684b43 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 09:27:05 +0200 Subject: Critical voltage now leads to a proper arming state transition --- src/modules/commander/commander.cpp | 10 +++++++--- src/modules/commander/state_machine_helper.cpp | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4e6ecd1e4..926be91b9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -812,8 +812,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; @@ -887,7 +885,13 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (armed.armed) { + // XXX not sure what should happen when voltage is low in flight + //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + } else { + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + } status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2..ef3890b71 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -168,6 +168,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * if (ret == TRANSITION_CHANGED) { status->arming_state = new_arming_state; arming_state_changed = true; + } else { + warnx("arming transition rejected"); } } -- cgit v1.2.3 From 02c23c785e5aa5d85f737a7735bc62355f945066 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 10:17:57 +0200 Subject: System status load is now from 0 to 1 instead of non-intuitive 0 to 1000 --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 926be91b9..fc7670ee5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -862,7 +862,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120..3d3434670 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -660,7 +660,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, + v_status.load * 1000.0f, v_status.battery_voltage * 1000.0f, v_status.battery_current * 1000.0f, v_status.battery_remaining, diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index e7feaa98e..4d49316c3 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -224,7 +224,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - float load; + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; float battery_remaining; -- cgit v1.2.3 From 2c6570cec803775feef1c1214cbe9236f05adde0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 10:20:04 +0200 Subject: Forgot load change in mavlink_onboard --- src/modules/mavlink_onboard/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index c5dbd00dd..e71344982 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -448,7 +448,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, + v_status.load * 1000.0f, v_status.battery_voltage * 1000.0f, v_status.battery_current * 1000.0f, v_status.battery_remaining, -- cgit v1.2.3 From 0fe612e843d6d0e7167c5ec4d33958c02efbbab6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 13:04:57 +0200 Subject: Simplified the RGBLED driver --- src/drivers/drv_rgbled.h | 27 +++-- src/drivers/rgbled/rgbled.cpp | 252 ++++++++++++++++++++---------------------- 2 files changed, 136 insertions(+), 143 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index f7cc5809a..66741e549 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -67,22 +67,26 @@ #define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) /** set constant RGB values */ -#define RGBLED_SET _RGBLEDIOC(4) +#define RGBLED_SET_RGB _RGBLEDIOC(4) /** set color */ #define RGBLED_SET_COLOR _RGBLEDIOC(5) +/** set blink pattern and speed */ +#define RGBLED_SET_MODE _RGBLEDIOC(6) + /* - structure passed to RGBLED_SET ioctl() + structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling */ -struct RGBLEDSet { +typedef struct { uint8_t red; uint8_t green; uint8_t blue; -}; +} rgbled_rgbset_t; +/* enum passed to RGBLED_SET_COLOR ioctl()*/ typedef enum { RGBLED_COLOR_OFF, RGBLED_COLOR_RED, @@ -91,13 +95,14 @@ typedef enum { RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, - RGBLED_COLOR_AMBER, + RGBLED_COLOR_AMBER } rgbled_color_t; +/* enum passed to RGBLED_SET_MODE ioctl()*/ typedef enum { - RGBLED_BLINK_ON, - RGBLED_BLINK_FAST, - RGBLED_BLINK_NORMAL, - RGBLED_BLINK_SLOW, - RGBLED_BLINK_OFF -} rgbled_blinkmode_t; + RGBLED_MODE_OFF, + RGBLED_MODE_ON, + RGBLED_MODE_BLINK_SLOW, + RGBLED_MODE_BLINK_NORMAL, + RGBLED_MODE_BLINK_FAST +} rgbled_mode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 35fdc5158..96b994888 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -77,13 +77,6 @@ #define SETTING_ENABLE 0x02 /**< on */ -enum ledModes { - RGBLED_MODE_TEST, - RGBLED_MODE_SYSTEMSTATE, - RGBLED_MODE_OFF, - RGBLED_MODE_RGB -}; - class RGBLED : public device::I2C { public: @@ -94,29 +87,29 @@ public: virtual int init(); virtual int probe(); virtual int info(); - virtual int setMode(enum ledModes mode); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: work_s _work; - rgbled_color_t _led_colors[8]; - rgbled_blinkmode_t _led_blinkmode; + rgbled_color_t _colors[8]; + rgbled_mode_t _mode; - // RGB values for MODE_RGB - struct RGBLEDSet _rgb; + bool _should_run; + bool _running; + int _led_interval; + int _counter; - int _mode; - int _running; + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); - void setLEDColor(rgbled_color_t ledcolor); static void led_trampoline(void *arg); void led(); - int set(bool on, uint8_t r, uint8_t g, uint8_t b); - int set_on(bool on); - int set_rgb(uint8_t r, uint8_t g, uint8_t b); - int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); + int set(bool on, uint8_t r, uint8_t g, uint8_t b); + int set_on(bool on); + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); }; /* for now, we only support one RGBLED */ @@ -130,10 +123,11 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - _led_colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), - _led_blinkmode(RGBLED_BLINK_OFF), + _colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), _mode(RGBLED_MODE_OFF), - _running(false) + _running(false), + _led_interval(0), + _counter(0) { memset(&_work, 0, sizeof(_work)); } @@ -158,35 +152,6 @@ RGBLED::init() return OK; } -int -RGBLED::setMode(enum ledModes new_mode) -{ - switch (new_mode) { - case RGBLED_MODE_SYSTEMSTATE: - case RGBLED_MODE_TEST: - case RGBLED_MODE_RGB: - _mode = new_mode; - if (!_running) { - _running = true; - set_on(true); - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); - } - break; - - case RGBLED_MODE_OFF: - - default: - if (_running) { - _running = false; - set_on(false); - } - _mode = RGBLED_MODE_OFF; - break; - } - - return OK; -} - int RGBLED::probe() { @@ -225,16 +190,24 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { - case RGBLED_SET: { + case RGBLED_SET_RGB: /* set the specified RGB values */ - memcpy(&_rgb, (struct RGBLEDSet *)arg, sizeof(_rgb)); - setMode(RGBLED_MODE_RGB); + rgbled_rgbset_t rgbset; + memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset)); + set_rgb(rgbset.red, rgbset.green, rgbset.blue); + set_mode(RGBLED_MODE_ON); return OK; - } - case RGBLED_SET_COLOR: { + + case RGBLED_SET_COLOR: /* set the specified color name */ - setLEDColor((rgbled_color_t)arg); - } + set_color((rgbled_color_t)arg); + return OK; + + case RGBLED_SET_MODE: + /* set the specified blink pattern/speed */ + set_mode((rgbled_mode_t)arg); + return OK; + default: break; @@ -257,77 +230,32 @@ RGBLED::led_trampoline(void *arg) void RGBLED::led() { - static int led_thread_runcount=0; - static int _led_interval = 1000; - switch (_mode) { - case RGBLED_MODE_TEST: - /* Demo LED pattern for now */ - _led_colors[0] = RGBLED_COLOR_YELLOW; - _led_colors[1] = RGBLED_COLOR_AMBER; - _led_colors[2] = RGBLED_COLOR_RED; - _led_colors[3] = RGBLED_COLOR_PURPLE; - _led_colors[4] = RGBLED_COLOR_BLUE; - _led_colors[5] = RGBLED_COLOR_GREEN; - _led_colors[6] = RGBLED_COLOR_WHITE; - _led_colors[7] = RGBLED_COLOR_OFF; - _led_blinkmode = RGBLED_BLINK_ON; - break; - - case RGBLED_MODE_SYSTEMSTATE: - /* XXX TODO set pattern */ - _led_colors[0] = RGBLED_COLOR_OFF; - _led_colors[1] = RGBLED_COLOR_OFF; - _led_colors[2] = RGBLED_COLOR_OFF; - _led_colors[3] = RGBLED_COLOR_OFF; - _led_colors[4] = RGBLED_COLOR_OFF; - _led_colors[5] = RGBLED_COLOR_OFF; - _led_colors[6] = RGBLED_COLOR_OFF; - _led_colors[7] = RGBLED_COLOR_OFF; - _led_blinkmode = RGBLED_BLINK_OFF; - + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if(_counter % 2 == 0) + set_on(true); + else + set_on(false); break; - - case RGBLED_MODE_RGB: - set_rgb(_rgb.red, _rgb.green, _rgb.blue); - _running = false; - return; - - case RGBLED_MODE_OFF: default: - return; break; } + _counter++; - if (led_thread_runcount & 1) { - if (_led_blinkmode == RGBLED_BLINK_ON) - setLEDColor(RGBLED_COLOR_OFF); - _led_interval = RGBLED_OFFTIME; - } else { - setLEDColor(_led_colors[(led_thread_runcount/2) % 8]); - _led_interval = RGBLED_ONTIME; - } - - led_thread_runcount++; - - if(_running) { - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); - } else if (_mode == RGBLED_MODE_RGB) { - // no need to run again until the colour changes - set_on(true); - } else { - set_on(false); - } + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); } -void RGBLED::setLEDColor(rgbled_color_t ledcolor) { - switch (ledcolor) { - case RGBLED_COLOR_OFF: // off +void +RGBLED::set_color(rgbled_color_t color) { + switch (color) { + case RGBLED_COLOR_OFF: // off set_rgb(0,0,0); break; - case RGBLED_COLOR_RED: // red + case RGBLED_COLOR_RED: // red set_rgb(255,0,0); break; case RGBLED_COLOR_YELLOW: // yellow @@ -339,7 +267,7 @@ void RGBLED::setLEDColor(rgbled_color_t ledcolor) { case RGBLED_COLOR_GREEN: // green set_rgb(0,255,0); break; - case RGBLED_COLOR_BLUE: // blue + case RGBLED_COLOR_BLUE: // blue set_rgb(0,0,255); break; case RGBLED_COLOR_WHITE: // white @@ -348,6 +276,52 @@ void RGBLED::setLEDColor(rgbled_color_t ledcolor) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; + default: + warnx("color unknown"); + break; + } +} + +void +RGBLED::set_mode(rgbled_mode_t mode) +{ + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + set_on(false); + break; + case RGBLED_MODE_ON: + _should_run = false; + set_on(true); + break; + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _led_interval = 2000; + break; + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _led_interval = 1000; + break; + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _led_interval = 500; + break; + default: + warnx("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + /* if it should stop, then cancel the workq */ + if (!_should_run && _running) { + _running = false; + work_cancel(LPWORK, &_work); } } @@ -417,7 +391,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'systemstate', 'test', 'info', 'off', 'rgb'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -446,6 +420,9 @@ rgbled_main(int argc, char *argv[]) argv += optind; const char *verb = argv[0]; + int fd; + int ret; + if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) errx(1, "already started"); @@ -480,19 +457,25 @@ rgbled_main(int argc, char *argv[]) /* need the driver past this point */ if (g_rgbled == nullptr) { - fprintf(stderr, "not started\n"); + warnx("not started"); rgbled_usage(); exit(0); } if (!strcmp(verb, "test")) { - g_rgbled->setMode(RGBLED_MODE_TEST); - exit(0); - } + fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + ret = ioctl(fd, RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_WHITE); - if (!strcmp(verb, "systemstate")) { - g_rgbled->setMode(RGBLED_MODE_SYSTEMSTATE); - exit(0); + if(ret != OK) { + close(fd); + exit(ret); + } + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL); + close(fd); + exit(ret); } if (!strcmp(verb, "info")) { @@ -501,23 +484,28 @@ rgbled_main(int argc, char *argv[]) } if (!strcmp(verb, "off")) { - g_rgbled->setMode(RGBLED_MODE_OFF); - exit(0); + fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + close(fd); + exit(ret); } if (!strcmp(verb, "rgb")) { - int fd = open(RGBLED_DEVICE_PATH, 0); + fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } if (argc < 4) { errx(1, "Usage: rgbled rgb "); } - struct RGBLEDSet v; + rgbled_rgbset_t v; v.red = strtol(argv[1], NULL, 0); v.green = strtol(argv[2], NULL, 0); v.blue = strtol(argv[3], NULL, 0); - int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v); + ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); close(fd); exit(ret); } -- cgit v1.2.3 From 63af0a80ee19a73a94a3b46bbddffe1b80610a3c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 15:08:10 +0200 Subject: multirotor_att_control: run rate controller only if vehicle_control_mode flag set, codestyle fixed --- .../multirotor_att_control_main.c | 43 ++++++++++++---------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 7014d22c4..65b19c8e9 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -242,7 +242,7 @@ mc_thread_main(int argc, char *argv[]) /* control attitude, update attitude setpoint depending on mode */ /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { + control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { att_sp.yaw_body = att.yaw; } @@ -305,6 +305,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } + control_yaw_position = true; } @@ -312,6 +313,7 @@ mc_thread_main(int argc, char *argv[]) /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_position_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; @@ -355,23 +357,24 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; - - /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + if (control_mode.flag_control_rates_enabled) { + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); - } + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } - /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + /* apply controller */ + float gyro[3]; + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; @@ -449,11 +452,11 @@ int multirotor_att_control_main(int argc, char *argv[]) thread_should_exit = false; mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); exit(0); } -- cgit v1.2.3 From 05e4c086cecf5fa13cac80d0d9724f1b6bac431c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 16:24:44 +0200 Subject: Added orientation support and detection to the L3GD20/H driver to support the different variants in use --- src/drivers/l3gd20/l3gd20.cpp | 55 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 51 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 42a0c264c..05739f04f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -71,6 +71,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -186,6 +192,7 @@ private: unsigned _current_rate; unsigned _current_range; + unsigned _orientation; perf_counter_t _sample_perf; @@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), + _orientation(SENSOR_BOARD_ROTATION_270_DEG), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _gyro_filter_x(250, 30), _gyro_filter_y(250, 30), @@ -363,8 +371,23 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #else + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif + return OK; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; return OK; + } return -EIO; } @@ -717,9 +740,33 @@ L3GD20::measure() */ report->timestamp = hrt_absolute_time(); - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report->x_raw = raw_report.x; + report->y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report->x_raw = raw_report.y; + report->y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -- cgit v1.2.3 From 1d7b8bb565a5450d30a6adc72b0130c5d03ba3be Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:03:04 +0200 Subject: Somehow alarm 14 didn't always work --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index f314b5876..b06920a76 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -456,9 +456,7 @@ const char * const ToneAlarm::_default_tunes[] = { "O2E2P64", "MNT75L1O2G", //arming warning "MBNT100a8", //battery warning slow - "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" - "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" - "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition + "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" //battery warning fast // XXX why is there a break before a repetition }; const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); -- cgit v1.2.3 From af3e0d459a018fe37d647d3089b4ea681d9244f4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:04:24 +0200 Subject: Add pattern ioctl for RGBLED --- src/drivers/drv_rgbled.h | 15 +++++++++++-- src/drivers/rgbled/rgbled.cpp | 52 ++++++++++++++++++++++++++++++++++--------- 2 files changed, 54 insertions(+), 13 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 66741e549..0f48f6f79 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -72,9 +72,12 @@ /** set color */ #define RGBLED_SET_COLOR _RGBLEDIOC(5) -/** set blink pattern and speed */ +/** set blink speed */ #define RGBLED_SET_MODE _RGBLEDIOC(6) +/** set pattern */ +#define RGBLED_SET_PATTERN _RGBLEDIOC(7) + /* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless @@ -104,5 +107,13 @@ typedef enum { RGBLED_MODE_ON, RGBLED_MODE_BLINK_SLOW, RGBLED_MODE_BLINK_NORMAL, - RGBLED_MODE_BLINK_FAST + RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_PATTERN } rgbled_mode_t; + +#define RGBLED_PATTERN_LENGTH 20 + +typedef struct { + rgbled_color_t color[RGBLED_PATTERN_LENGTH]; + unsigned duration[RGBLED_PATTERN_LENGTH]; +} rgbled_pattern_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 96b994888..858be8058 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -92,8 +92,9 @@ public: private: work_s _work; - rgbled_color_t _colors[8]; + rgbled_color_t _color; rgbled_mode_t _mode; + rgbled_pattern_t _pattern; bool _should_run; bool _running; @@ -102,6 +103,7 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); static void led_trampoline(void *arg); void led(); @@ -123,13 +125,14 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - _colors({RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF,RGBLED_COLOR_OFF}), + _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), _running(false), _led_interval(0), _counter(0) { memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); } RGBLED::~RGBLED() @@ -204,10 +207,14 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case RGBLED_SET_MODE: - /* set the specified blink pattern/speed */ + /* set the specified blink speed */ set_mode((rgbled_mode_t)arg); return OK; + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t*)arg); + return OK; default: break; @@ -239,6 +246,14 @@ RGBLED::led() else set_on(false); break; + case RGBLED_MODE_PATTERN: + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + _counter = 0; + + set_color(_pattern.color[_counter]); + _led_interval = _pattern.duration[_counter]; + break; default: break; } @@ -251,6 +266,9 @@ RGBLED::led() void RGBLED::set_color(rgbled_color_t color) { + + _color = color; + switch (color) { case RGBLED_COLOR_OFF: // off set_rgb(0,0,0); @@ -302,11 +320,16 @@ RGBLED::set_mode(rgbled_mode_t mode) break; case RGBLED_MODE_BLINK_NORMAL: _should_run = true; - _led_interval = 1000; + _led_interval = 500; break; case RGBLED_MODE_BLINK_FAST: _should_run = true; - _led_interval = 500; + _led_interval = 100; + break; + case RGBLED_MODE_PATTERN: + _should_run = true; + set_on(true); + _counter = 0; break; default: warnx("mode unknown"); @@ -325,6 +348,14 @@ RGBLED::set_mode(rgbled_mode_t mode) } } +void +RGBLED::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); + + set_mode(RGBLED_MODE_PATTERN); +} + int RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) { @@ -467,13 +498,12 @@ rgbled_main(int argc, char *argv[]) if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - ret = ioctl(fd, RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_WHITE); - if(ret != OK) { - close(fd); - exit(ret); - } - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL); + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF}, + {200, 200, 200, 400 } }; + + ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + close(fd); exit(ret); } -- cgit v1.2.3 From 451adf2aa0d9795f69f5675b00ff3fb245312eb0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:05:59 +0200 Subject: Added part of RGBLED stuff to commander --- src/modules/commander/commander.cpp | 114 ++++++++++++++++++++++------- src/modules/commander/commander_helper.cpp | 27 +++++-- src/modules/commander/commander_helper.h | 9 ++- 3 files changed, 116 insertions(+), 34 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fc7670ee5..2e5d080b9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -197,7 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -698,6 +698,8 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { hrt_abstime t = hrt_absolute_time(); + status_changed = false; + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -855,8 +857,6 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - toggle_status_leds(&status, &armed, &gps_position); - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -867,14 +867,17 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } + warnx("bat remaining: %2.2f", status.battery_remaining); + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; status_changed = true; + battery_tune_played = false; } low_voltage_counter++; @@ -885,12 +888,14 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + battery_tune_played = false; if (armed.armed) { // XXX not sure what should happen when voltage is low in flight //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + // XXX should we still allow to arm with critical battery? + //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; } @@ -1259,7 +1264,6 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - status_changed = false; } /* play arming and battery warning tunes */ @@ -1273,7 +1277,7 @@ int commander_thread_main(int argc, char *argv[]) if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1294,6 +1298,9 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; + + toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1325,40 +1332,93 @@ int commander_thread_main(int argc, char *argv[]) } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { - if (leds_counter % 2 == 0) { - /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_BLUE); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 1Hz */ + if (leds_counter % 20 == 0) + led_toggle(LED_BLUE); + } else { + /* not ready to arm, blink at 10Hz */ + if (leds_counter % 2 == 0) + led_toggle(LED_BLUE); + } +#endif + + if (changed) { + + warnx("changed"); + + int i; + rgbled_pattern_t pattern; + memset(&pattern, 0, sizeof(pattern)); + if (armed->armed) { /* armed, solid */ - led_on(LED_AMBER); + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[0] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[0] = RGBLED_COLOR_RED; + } else { + pattern.color[0] = RGBLED_COLOR_GREEN; + } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + for (i=0; i<3; i++) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[i*2] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[i*2] = RGBLED_COLOR_RED; + } else { + pattern.color[i*2] = RGBLED_COLOR_GREEN; + } + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; + } + if (status->condition_global_position_valid) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 1000; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; } else { - led_toggle(LED_AMBER); + for (i=3; i<6; i++) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 100; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 100; + } + pattern.color[6*2] = RGBLED_COLOR_OFF; + pattern.duration[6*2] = 700; } } else { + for (i=0; i<3; i++) { + pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 200; + } /* not ready to arm, blink at 10Hz */ - led_toggle(LED_AMBER); } - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 0) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); + rgbled_set_pattern(&pattern); + } - } else { - /* no position estimator available, off */ - led_off(LED_BLUE); - } + /* give system warnings on error LED, XXX maybe add memory usage warning too */ + if (status->load > 0.95f) { + if (leds_counter % 2 == 0) + led_toggle(LED_AMBER); + } else { + led_off(LED_AMBER); } leds_counter++; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d9b255f4f..5df5d8d0c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -122,16 +122,18 @@ int tune_arm() return ioctl(buzzer, TONE_SET_ALARM, 12); } -int tune_critical_bat() +int tune_low_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 14); + return ioctl(buzzer, TONE_SET_ALARM, 13); } -int tune_low_bat() +int tune_critical_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 13); + return ioctl(buzzer, TONE_SET_ALARM, 14); } + + void tune_stop() { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -201,9 +203,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -int rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); +} + +void rgbled_set_mode(rgbled_mode_t mode) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); +} + +void rgbled_set_pattern(rgbled_pattern_t *pattern) { - return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color); + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage) diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index c621b9823..027d0535f 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -45,6 +45,7 @@ #include #include #include +#include bool is_multirotor(const struct vehicle_status_s *current_status); @@ -58,8 +59,8 @@ void tune_positive(void); void tune_neutral(void); void tune_negative(void); int tune_arm(void); -int tune_critical_bat(void); int tune_low_bat(void); +int tune_critical_bat(void); void tune_stop(void); int led_init(void); @@ -68,6 +69,12 @@ int led_toggle(int led); int led_on(int led); int led_off(int led); +void rgbled_set_color(rgbled_color_t color); + +void rgbled_set_mode(rgbled_mode_t mode); + +void rgbled_set_pattern(rgbled_pattern_t *pattern); + /** * Provides a coarse estimate of remaining battery power. * -- cgit v1.2.3 From 4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 21:24:19 +0200 Subject: position_estimator_inav: fixed global_pos topic publishing --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d4b2f0e43..0530c2dea 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,6 +116,7 @@ int position_estimator_inav_main(int argc, char *argv[]) } verbose_mode = false; + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -375,6 +376,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -551,9 +553,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (params.use_gps) { - global_pos.valid = local_pos.valid; double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + + global_pos.valid = local_pos.valid; + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.alt = local_pos.home_alt - local_pos.z; -- cgit v1.2.3 From c543f89ec17048c1b5264623a885a9247a05304c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 23:36:49 +0200 Subject: commander, multirotor_pos_control, multirotor_att_control: bugfixes --- src/modules/commander/commander.cpp | 132 ++++++++++++--------- src/modules/commander/state_machine_helper.cpp | 89 +++++++------- .../multirotor_att_control_main.c | 8 +- .../multirotor_pos_control.c | 22 ++-- src/modules/uORB/topics/vehicle_control_mode.h | 3 +- src/modules/uORB/topics/vehicle_local_position.h | 1 + 6 files changed, 134 insertions(+), 121 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6181dafab..872939d6d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -122,7 +122,7 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + case VEHICLE_CMD_NAV_TAKEOFF: { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } break; + } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { + if (is_safe(status, safety, armed)) { - if (((int)(cmd->param1)) == 1) { - /* reboot */ - up_systemreset(); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value /* armed topic */ struct actuator_armed_s armed; @@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[]) last_diff_pres_time = diff_pres.timestamp; } + /* Check for valid airspeed/differential pressure measurements */ + if (t - last_diff_pres_time < 2000000 && t > 2000000) { + status.condition_airspeed_valid = true; + + } else { + status.condition_airspeed_valid = false; + } + orb_check(cmd_sub, &updated); if (updated) { @@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* set the condition to valid if there has recently been a global position estimate */ + if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { + if (!status.condition_global_position_valid) { + status.condition_global_position_valid = true; + status_changed = true; + } + + } else { + if (status.condition_global_position_valid) { + status.condition_global_position_valid = false; + status_changed = true; + } + } + /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { if (!status.condition_local_position_valid) { status.condition_local_position_valid = true; status_changed = true; @@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; - - } else { - status.condition_global_position_valid = false; - } - - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; - - } else { - status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } - orb_check(gps_sub, &updated); if (updated) { @@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { /* DENIED here indicates safety switch not pressed */ @@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; } else { - /* if not landed: act depending on switches */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2..f313adce4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ - { + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s // 3) Safety switch is present AND engaged -> actuators locked if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; + } else { return false; } @@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; @@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; @@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; case NAVIGATION_STATE_AUTO_READY: ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } break; case NAVIGATION_STATE_AUTO_LOITER: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_RTL: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_LAND: @@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 65b19c8e9..1aa24a4fc 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -250,12 +250,12 @@ mc_thread_main(int argc, char *argv[]) /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - if (!control_mode.flag_control_altitude_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* Don't touch throttle in modes with altitude hold, it's handled by position controller. * * Only go to failsafe throttle if last known throttle was @@ -309,12 +309,12 @@ mc_thread_main(int argc, char *argv[]) control_yaw_position = true; } - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0120fa61c..1cb470240 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -221,10 +221,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime home_alt_t = 0; uint64_t local_home_timestamp = 0; - static PID_t xy_pos_pids[2]; - static PID_t xy_vel_pids[2]; - static PID_t z_pos_pid; - static thrust_pid_t z_vel_pid; + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; thread_running = true; @@ -238,7 +238,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -247,9 +247,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz*/ - if (--paramcheck_counter <= 0) { - paramcheck_counter = 50; + /* check parameters at 1 Hz */ + if (++paramcheck_counter >= 50) { + paramcheck_counter = 0; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -441,14 +441,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subcrive to velocity setpoint if altitude/position control disabled - if (control_mode.flag_control_velocity_enabled) { + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_altitude_enabled) { + if (control_mode.flag_control_climb_rate_enabled) { thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } - if (control_mode.flag_control_position_enabled) { + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index d83eb45d9..fe169c6e6 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -75,9 +75,10 @@ struct vehicle_control_mode_s //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd..9d3b4468c 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -75,6 +75,7 @@ struct vehicle_local_position_s float home_alt; /**< Altitude in meters LOGME */ float home_hdg; /**< Compass heading in radians -PI..+PI. */ + bool landed; /**< true if vehicle is landed */ }; /** -- cgit v1.2.3 From 7aeaf10ae832af01bb3a1b511df0bae34a42bf89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 16:24:44 +0200 Subject: Added orientation support and detection to the L3GD20/H driver to support the different variants in use --- src/drivers/l3gd20/l3gd20.cpp | 55 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 51 insertions(+), 4 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 42a0c264c..05739f04f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -71,6 +71,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -186,6 +192,7 @@ private: unsigned _current_rate; unsigned _current_range; + unsigned _orientation; perf_counter_t _sample_perf; @@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), + _orientation(SENSOR_BOARD_ROTATION_270_DEG), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _gyro_filter_x(250, 30), _gyro_filter_y(250, 30), @@ -363,8 +371,23 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #else + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif + return OK; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; return OK; + } return -EIO; } @@ -717,9 +740,33 @@ L3GD20::measure() */ report->timestamp = hrt_absolute_time(); - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report->x_raw = raw_report.x; + report->y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report->x_raw = raw_report.y; + report->y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; -- cgit v1.2.3 From 2be5240b9f70803417c9648133490409ba40ba55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 Aug 2013 12:37:41 +0200 Subject: commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup --- src/modules/commander/commander.cpp | 124 +++++++++------------ .../multirotor_att_control_main.c | 20 ++-- .../position_estimator_inav_main.c | 15 ++- 3 files changed, 74 insertions(+), 85 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 872939d6d..d40f6675b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,12 +117,9 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define RC_TIMEOUT 100000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -197,6 +194,8 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); +void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -354,19 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; } - break; - } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { @@ -713,8 +714,6 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { - hrt_abstime t = hrt_absolute_time(); - /* update parameters */ orb_check(param_changed_sub, &updated); @@ -773,16 +772,9 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); orb_check(cmd_sub, &updated); @@ -809,19 +801,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } - /* set the condition to valid if there has recently been a global position estimate */ - if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { - if (!status.condition_global_position_valid) { - status.condition_global_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_global_position_valid) { - status.condition_global_position_valid = false; - status_changed = true; - } - } + /* update condition_global_position_valid */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -831,19 +812,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { - if (!status.condition_local_position_valid) { - status.condition_local_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_local_position_valid) { - status.condition_local_position_valid = false; - status_changed = true; - } - } + /* update condition_local_position_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -854,7 +824,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -981,10 +951,9 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + if (!home_position_set && gps_position.fix_type >= 3 && (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (t - gps_position.timestamp_position < 2000000) - && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate home.lat = gps_position.lat; @@ -1019,7 +988,7 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { /* start RC input check */ - if ((t - sp_man.timestamp) < 100000) { + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1123,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { // TODO remove debug code if (!status.rc_signal_cutting_off) { warnx("Reason: not rc_signal_cutting_off\n"); @@ -1136,11 +1105,11 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = t - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { @@ -1157,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO check this /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1213,23 +1182,23 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = t; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; status_changed = true; } @@ -1256,9 +1225,11 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + hrt_abstime t1 = hrt_absolute_time(); + /* publish arming state */ if (arming_state_changed) { - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1266,14 +1237,14 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_changed) { /* publish new navigation state */ control_mode.counter++; - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } /* publish vehicle status at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; - status.timestamp = t; + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); status_changed = false; } @@ -1340,6 +1311,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) { @@ -1367,7 +1350,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp /* position estimated, solid */ led_on(LED_BLUE); - } else if (leds_counter == 0) { + } else if (leds_counter == 6) { /* waiting for position estimate, short blink at 1.25Hz */ led_on(LED_BLUE); @@ -1548,6 +1531,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 1aa24a4fc..5cad667f6 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -367,14 +367,20 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - float gyro[3]; - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators); + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0530c2dea..0e7e0ef5d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,6 +75,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; if (can_estimate_pos) { /* inertial filter prediction for position */ @@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); - } + if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } -- cgit v1.2.3 From b71c0c1f491fc91561393c1ff1c1646b251fd96e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:46:13 +0200 Subject: Added support for FMUv1 for RGB led and dim led support --- makefiles/config_px4fmu-v1_default.mk | 1 + src/drivers/boards/px4fmu-v1/board_config.h | 2 ++ src/drivers/drv_rgbled.h | 9 ++++++++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d7d9a1ecb..8b4ea18bf 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -30,6 +30,7 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/md25 MODULES += drivers/airspeed diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 9d7c81f85..27621211a 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -103,6 +103,7 @@ __BEGIN_DECLS #define PX4_I2C_BUS_ESC 1 #define PX4_I2C_BUS_ONBOARD 2 #define PX4_I2C_BUS_EXPANSION 3 +#define PX4_I2C_BUS_LED 3 /* * Devices on the onboard bus. @@ -112,6 +113,7 @@ __BEGIN_DECLS #define PX4_I2C_OBDEV_HMC5883 0x1e #define PX4_I2C_OBDEV_MS5611 0x76 #define PX4_I2C_OBDEV_EEPROM NOTDEFINED +#define PX4_I2C_OBDEV_LED 0x55 #define PX4_I2C_OBDEV_PX4IO_BL 0x18 #define PX4_I2C_OBDEV_PX4IO 0x1a diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 0f48f6f79..3c8bdec5d 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -98,7 +98,14 @@ typedef enum { RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, - RGBLED_COLOR_AMBER + RGBLED_COLOR_AMBER, + RGBLED_COLOR_DIM_RED, + RGBLED_COLOR_DIM_YELLOW, + RGBLED_COLOR_DIM_PURPLE, + RGBLED_COLOR_DIM_GREEN, + RGBLED_COLOR_DIM_BLUE, + RGBLED_COLOR_DIM_WHITE, + RGBLED_COLOR_DIM_AMBER } rgbled_color_t; /* enum passed to RGBLED_SET_MODE ioctl()*/ -- cgit v1.2.3 From 64145252d4133b6df36229f548d02a692c3ec6fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:46:34 +0200 Subject: Added dim RGB implementation --- src/drivers/rgbled/rgbled.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 858be8058..f2543a33c 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -294,6 +294,27 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; + case RGBLED_COLOR_DIM_RED: // red + set_rgb(90,0,0); + break; + case RGBLED_COLOR_DIM_YELLOW: // yellow + set_rgb(80,30,0); + break; + case RGBLED_COLOR_DIM_PURPLE: // purple + set_rgb(45,0,45); + break; + case RGBLED_COLOR_DIM_GREEN: // green + set_rgb(0,90,0); + break; + case RGBLED_COLOR_DIM_BLUE: // blue + set_rgb(0,0,90); + break; + case RGBLED_COLOR_DIM_WHITE: // white + set_rgb(30,30,30); + break; + case RGBLED_COLOR_DIM_AMBER: // amber + set_rgb(80,20,0); + break; default: warnx("color unknown"); break; -- cgit v1.2.3 From 6c45d9cb5cc4e9efb99aff84a1fe10f084a998c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:47:13 +0200 Subject: Fixed in-air timout, bumped protocol version --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/protocol.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3aecd9b69..045b3ebcb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -508,7 +508,7 @@ PX4IO::init() usleep(10000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 3000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 02df76068..97809d9c2 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -74,7 +74,7 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) -#define PX4IO_PROTOCOL_VERSION 2 +#define PX4IO_PROTOCOL_VERSION 3 /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -- cgit v1.2.3 From 6ff3f51f88c2335f225a2a391de5ee353487a69b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:47:42 +0200 Subject: Added dim RGB led support, not operational yet --- src/modules/commander/commander.cpp | 35 +++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2e5d080b9..30906034e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; +/* if connected via USB */ +static bool on_usb_power = false; /* tasks waiting for low prio thread */ @@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st void print_reject_mode(const char *msg); +void print_status(); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\tcommander is running\n"); + print_status(); } else { warnx("\tcommander not started\n"); @@ -265,6 +271,10 @@ void usage(const char *reason) exit(1); } +void print_status() { + warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); +} + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -865,9 +875,14 @@ int commander_thread_main(int argc, char *argv[]) status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; + + /* check if board is connected via USB */ + struct stat statbuf; + //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - warnx("bat remaining: %2.2f", status.battery_remaining); + // XXX remove later + //warnx("bat remaining: %2.2f", status.battery_remaining); /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { @@ -1362,22 +1377,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[0] = RGBLED_COLOR_AMBER; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[0] = RGBLED_COLOR_RED; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { - pattern.color[0] = RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; } pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { for (i=0; i<3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = RGBLED_COLOR_AMBER; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { - pattern.color[i*2] = RGBLED_COLOR_GREEN; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } pattern.duration[i*2] = 200; @@ -1385,13 +1400,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang pattern.duration[i*2+1] = 800; } if (status->condition_global_position_valid) { - pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; pattern.duration[i*2] = 1000; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 800; } else { for (i=3; i<6; i++) { - pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; pattern.duration[i*2] = 100; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 100; @@ -1402,7 +1417,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { for (i=0; i<3; i++) { - pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; pattern.duration[i*2] = 200; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 200; -- cgit v1.2.3 From e9b6cfd671c72b75f2bf79bf1031ea8697f430b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:48:13 +0200 Subject: Fixed startup order of F330 script --- ROMFS/px4fmu_common/init.d/10_io_f330 | 18 +++++------------- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++++++ 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index b3fb02757..0634d650e 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -44,6 +44,11 @@ param set MAV_TYPE 2 # mavlink start usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start # # Start PX4IO interface (depends on orb, commander) @@ -77,11 +82,6 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors - -# -# Start the commander (depends on orb, mavlink) -# -commander start # # Start GPS interface (depends on orb) @@ -102,15 +102,7 @@ if [ $BOARD == fmuv1 ] then px4io limit 200 sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi else px4io limit 400 sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6..60cc13611 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -81,6 +81,19 @@ else fi fi +# +# Start system state indicator +# +if rgbled start +then + echo "Using external RGB Led" +else + if blinkm start + then + blinkm systemstate + fi +fi + # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) # -- cgit v1.2.3 From 74802f0692ad61ef3995b2f05c7af043ab9fcaf3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 17 Aug 2013 18:01:51 +0200 Subject: Added possibility to set board orientation --- src/modules/sensors/sensor_params.c | 3 + src/modules/sensors/sensors.cpp | 148 +++++++++++++++++++++++++++++++++--- 2 files changed, 141 insertions(+), 10 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 255dfed18..8d3992963 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42268b971..7299b21bc 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -137,6 +138,77 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +/** + * Enum for board and external compass rotations + * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct +{ + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = +{ + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + /** * Sensor app start / stop handling function * @@ -210,6 +282,9 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -227,6 +302,9 @@ private: float accel_scale[3]; float diff_pres_offset_pa; + int board_rotation; + int external_mag_rotation; + int rc_type; int rc_map_roll; @@ -306,6 +384,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -314,6 +395,11 @@ private: */ int parameters_update(); + /** + * Get the rotation matrices + */ + void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + /** * Do accel-related initialisation. */ @@ -450,7 +536,10 @@ Sensors::Sensors() : _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3,3), + _external_mag_rotation(3,3) { /* basic r/c parameters */ @@ -540,6 +629,10 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -731,9 +824,33 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } +void +Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3,3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + (*rot_matrix)(i,j) = R(i, j); +} + void Sensors::accel_init() { @@ -874,9 +991,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation*vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -897,9 +1017,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation*vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -920,9 +1043,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + // XXX TODO add support for external mag orientation + + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + vect = _board_rotation*vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; -- cgit v1.2.3 From 36d474bfa31a44c49647cf8fc9d825cb1e919182 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:39:46 +0200 Subject: WIP on getting low-priority non-command tasks out of the commander main loop --- src/modules/commander/commander.cpp | 288 +++++++++++++++++++----------------- 1 file changed, 149 insertions(+), 139 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034e..891dd893b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -280,6 +280,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* only handle high-priority commands here */ + /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { @@ -363,95 +365,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // XXX implement later + result = VEHICLE_CMD_RESULT_DENIED; break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { - - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ - - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - break; - - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if ((int)(cmd->param1) == 1) { - /* gyro calibration */ - new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - - } else if ((int)(cmd->param2) == 1) { - /* magnetometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - - } else if ((int)(cmd->param3) == 1) { - /* zero-altitude pressure calibration */ - //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - } else if ((int)(cmd->param4) == 1) { - /* RC calibration */ - new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - - } else if ((int)(cmd->param5) == 1) { - /* accelerometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - - } else if ((int)(cmd->param6) == 1) { - /* airspeed calibration */ - new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if (((int)(cmd->param1)) == 0) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - - } else if (((int)(cmd->param1)) == 1) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - default: break; } @@ -460,6 +377,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* we do not care in the high prio loop about commands we don't know */ } else { tune_negative(); @@ -472,19 +391,24 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); } } /* send any requested ACKs */ - if (cmd->confirmation > 0) { + if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } } +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -524,13 +448,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); /* armed topic */ - struct actuator_armed_s armed; orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); @@ -631,7 +553,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; memset(&safety, 0, sizeof(safety)); safety.safety_switch_available = false; safety.safety_off = false; @@ -1633,80 +1554,169 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); - low_prio_task = LOW_PRIO_TASK_NONE; + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = cmd_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { - switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - } + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - case LOW_PRIO_TASK_PARAM_SAVE: + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + continue; - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - } + /* only handle low-priority commands here */ + switch (cmd.command) { - low_prio_task = LOW_PRIO_TASK_NONE; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + /* reboot to bootloader */ + systemreset(true); + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - do_gyro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + /* try to go to INIT/PREFLIGHT arming state */ - case LOW_PRIO_TASK_MAG_CALIBRATION: + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + result = VEHICLE_CMD_RESULT_DENIED; + break; + } - do_mag_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + do_gyro_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + do_mag_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - // do_baro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + result = VEHICLE_CMD_RESULT_DENIED; + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + result = VEHICLE_CMD_RESULT_DENIED; - case LOW_PRIO_TASK_RC_CALIBRATION: + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + do_accel_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - // do_rc_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + do_airspeed_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - do_accel_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + break; + } - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - do_airspeed_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + } + + break; + } - case LOW_PRIO_TASK_NONE: default: - /* slow down to 10Hz */ - usleep(100000); break; } + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + + } else { + tune_negative(); + + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + } + } + + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } + } return 0; -- cgit v1.2.3 From eda528157a04185cbb1342c152c4ac715f67771c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:40:28 +0200 Subject: Make state updates atomic (just to be really, really sure) --- src/modules/commander/state_machine_helper.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ef3890b71..5842a33b1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -69,6 +69,11 @@ static bool navigation_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ @@ -168,11 +173,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * if (ret == TRANSITION_CHANGED) { status->arming_state = new_arming_state; arming_state_changed = true; - } else { - warnx("arming transition rejected"); } } + /* end of atomic state update */ + irqrestore(flags); + + if (ret == TRANSITION_DENIED) + warnx("arming transition rejected"); + return ret; } -- cgit v1.2.3 From 628e806df5b05ea8a44d46f29606db03fee1fce9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:48:14 +0200 Subject: Minor style changes --- src/modules/sensors/sensors.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7299b21bc..c47f6bb7d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -139,8 +139,8 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** - * Enum for board and external compass rotations - * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. */ enum Rotation { ROTATION_NONE = 0, @@ -261,8 +261,8 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -282,8 +282,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ struct { float min[_rc_max_chan_count]; @@ -291,7 +291,6 @@ private: float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -343,7 +342,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; param_t rc_type; param_t rc_demix; @@ -874,7 +872,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -882,6 +880,9 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); + #else + #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif warnx("using system accel"); -- cgit v1.2.3 From 408eaf0ad1c8aa87c74f83281de3a7c25fc4b4e6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:22:40 +0200 Subject: Add ioctl to find out if mag is external or onboard --- src/drivers/drv_mag.h | 3 +++ src/drivers/hmc5883/hmc5883.cpp | 24 +++++++++++++++++++----- src/drivers/lsm303d/lsm303d.cpp | 15 ++++++++++++--- 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 9aab995a1..e95034e8e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag); /** perform self test and report status */ #define MAGIOCSELFTEST _MAGIOC(7) +/** determine if mag is external or onboard */ +#define MAGIOCGEXTERNAL _MAGIOC(8) + #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 039b496f4..9e9c067d5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -167,6 +167,8 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ + int _bus; /**< the bus the device is connected to */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -326,7 +328,8 @@ HMC5883::HMC5883(int bus) : _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), - _calibrated(false) + _calibrated(false), + _bus(bus) { // enable debug() calls _debug_enabled = false; @@ -665,6 +668,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return check_calibration(); + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) + return 1; + else + return 0; + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -851,12 +860,12 @@ HMC5883::collect() _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* XXX axis assignment of external sensor is yet unknown */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag seems to be rolled 180deg, therefore y and z inverted */ + _reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif @@ -1293,6 +1302,11 @@ test() warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* set the queue depth to 10 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 117583faf..3e6ce64b8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) // return self_test(); return -EINVAL; + case MAGIOCGEXTERNAL: + /* no external mag board yet */ + return 0; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -1422,7 +1426,7 @@ test() int fd_accel = -1; struct accel_report accel_report; ssize_t sz; - int filter_bandwidth; + int ret; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1445,10 +1449,10 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else - warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); + warnx("accel antialias filter bandwidth: %d Hz", ret); int fd_mag = -1; struct mag_report m_report; @@ -1459,6 +1463,11 @@ test() if (fd_mag < 0) err(1, "%s open failed", MAG_DEVICE_PATH); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); -- cgit v1.2.3 From bc717f1715590a6915c223dde53fe6e1139f92d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:33:59 +0200 Subject: Sensors should now take into account the orientation of an external mag --- src/modules/sensors/sensors.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c47f6bb7d..198da9f0a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -284,6 +284,7 @@ private: math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ struct { float min[_rc_max_chan_count]; @@ -537,7 +538,8 @@ Sensors::Sensors() : _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _board_rotation(3,3), - _external_mag_rotation(3,3) + _external_mag_rotation(3,3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -948,6 +950,14 @@ Sensors::mag_init() /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; + close(fd); } @@ -1044,10 +1054,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - // XXX TODO add support for external mag orientation - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; - vect = _board_rotation*vect; + + if (_mag_is_external) + vect = _external_mag_rotation*vect; + else + vect = _board_rotation*vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); -- cgit v1.2.3 From e5f1a7c2c3a67648829ec0dff5bf290dddc25847 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 12:42:24 +0200 Subject: Better transparency in IO mode display, fixes to commander arming, minimum publishing rate for arming --- src/drivers/px4io/px4io.cpp | 4 +- src/modules/commander/commander.cpp | 51 +++++++++++++++++++++++--- src/modules/commander/state_machine_helper.cpp | 2 +- 3 files changed, 49 insertions(+), 8 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8318238e2..0f90db858 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1519,8 +1519,8 @@ PX4IO::print_status() uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034e..12543800e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -273,6 +273,43 @@ void usage(const char *reason) void print_status() { warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + + /* read all relevant states */ + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + struct vehicle_status_s state; + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + + const char* armed_str; + + switch (state.arming_state) { + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + default: + armed_str = "ERR: UNKNOWN STATE"; + break; + } + + + warnx("arming: %s", armed_str); } void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -945,9 +982,9 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; + // bool global_pos_valid = status.condition_global_position_valid; + // bool local_pos_valid = status.condition_local_position_valid; + // bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ @@ -1274,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } - /* publish vehicle status at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* play arming and battery warning tunes */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5842a33b1..1e31573d6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -119,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * { ret = TRANSITION_CHANGED; armed->armed = true; - armed->ready_to_arm = false; + armed->ready_to_arm = true; } break; -- cgit v1.2.3 From 57c5240f0283e2f3e325287f46ec87bd790122d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 13:57:21 +0200 Subject: Make a distinctive sound when the IO start fails (e.g. due to version mismatch) --- ROMFS/px4fmu_common/init.d/10_io_f330 | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 0634d650e..48636292c 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -53,8 +53,13 @@ commander start # # Start PX4IO interface (depends on orb, commander) # -px4io start -pwm -u 400 -m 0xff +if px4io start +then + pwm -u 400 -m 0xff +else + # SOS + tone_alarm 6 +fi # # Allow PX4IO to recover from midair restarts. -- cgit v1.2.3 From 80f38e3dea6b707314b407c7c511c19aa4eade39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 21:00:47 +0200 Subject: Put console and syslog on UART8, added support to nshterm for proper serial port config --- ROMFS/px4fmu_common/init.d/rcS | 9 +++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 ++ nuttx-configs/px4fmu-v2/nsh/defconfig | 14 +++++++------- src/systemcmds/nshterm/nshterm.c | 31 ++++++++++++++++++++++++++++--- 5 files changed, 47 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6..e06d90d1c 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -58,6 +58,15 @@ fi if [ $MODE == autostart ] then +# +# Start terminal +# +if sercon +then + echo "USB connected" + nshterm /dev/ttyACM0 & +fi + # # Start the ORB (first app to start) # diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index db13cc197..8f2ade8dc 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -54,6 +54,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index cc182e6af..101b49712 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -52,6 +52,8 @@ MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 85bf6dd2f..3e2a48108 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -420,7 +420,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=8 @@ -523,7 +523,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_DEV_LOWCONSOLE=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -542,8 +542,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_UART7_SERIAL_CONSOLE is not set -# CONFIG_UART8_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -650,7 +650,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -716,10 +716,10 @@ CONFIG_FS_BINFS=y # # System Logging # -# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG_ENABLE=y CONFIG_SYSLOG=y CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" +CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # # Graphics Support diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index bde0e7841..2341068a2 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -48,6 +49,7 @@ #include #include #include +#include __EXPORT int nshterm_main(int argc, char *argv[]); @@ -61,8 +63,8 @@ nshterm_main(int argc, char *argv[]) uint8_t retries = 0; int fd = -1; while (retries < 5) { - // the retries are to cope with the behaviour of /dev/ttyACM0 - // which may not be ready immediately. + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { break; @@ -74,7 +76,30 @@ nshterm_main(int argc, char *argv[]) perror(argv[1]); exit(1); } - // setup standard file descriptors + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ close(0); close(1); close(2); -- cgit v1.2.3 From ffc2a8b7a358a2003e5ed548c41878b33e7aa726 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 18 Aug 2013 23:05:26 +0200 Subject: vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now. --- src/modules/commander/commander.cpp | 96 +++++----- src/modules/commander/state_machine_helper.cpp | 27 +-- .../multirotor_pos_control.c | 20 +- .../position_estimator_inav_main.c | 201 +++++++++++---------- .../position_estimator_inav_params.c | 3 - .../position_estimator_inav_params.h | 2 - src/modules/sdlog2/sdlog2.c | 7 +- src/modules/sdlog2/sdlog2_messages.h | 9 +- src/modules/uORB/topics/vehicle_local_position.h | 39 ++-- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 195 insertions(+), 213 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d40f6675b..ab7d2e6cf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&control_mode, 0, sizeof(control_mode)); status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_STANDBY; + status.navigation_state = NAVIGATION_STATE_DIRECT; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -812,8 +812,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* update condition_local_position_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); + /* update condition_local_position_valid and condition_local_altitude_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -1512,68 +1513,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v { transition_result_t res = TRANSITION_DENIED; - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - /* ARMED */ - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); + break; - case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + case MAIN_STATE_AUTO: + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + + } else { + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } - - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - - break; - - default: - break; } - } else { - /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); + break; + + default: + break; } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f313adce4..76c7eaf92 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: - /* need position estimate */ - // TODO only need altitude estimate really - if (current_state->condition_local_position_valid) { + /* need altitude estimate */ + if (current_state->condition_local_altitude_valid) { ret = TRANSITION_CHANGED; } @@ -227,7 +226,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need position estimate */ + /* need local position estimate */ if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -236,8 +235,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: - /* need position estimate */ - if (current_state->condition_local_position_valid) { + /* need global position estimate */ + if (current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } else { switch (new_navigation_state) { - case NAVIGATION_STATE_STANDBY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - break; - case NAVIGATION_STATE_DIRECT: ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; @@ -394,9 +382,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ case NAVIGATION_STATE_AUTO_LAND: - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + /* deny transitions from landed state */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1cb470240..80ce33cda 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; - uint64_t local_home_timestamp = 0; + uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, project global setpoints to local frame */ //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.home_timestamp != local_home_timestamp) { - local_home_timestamp = local_pos.home_timestamp; + if (local_pos.ref_timestamp != local_ref_timestamp) { + local_ref_timestamp = local_pos.ref_timestamp; /* init local projection using local position home */ - double lat_home = local_pos.home_lat * 1e-7; - double lon_home = local_pos.home_lon * 1e-7; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); @@ -338,7 +338,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = -global_pos_sp.altitude; } else { - local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -355,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.home_timestamp != home_alt_t) { + if (local_pos.ref_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - home_alt; } - home_alt_t = local_pos.home_timestamp; - home_alt = local_pos.home_alt; + home_alt_t = local_pos.ref_timestamp; + home_alt = local_pos.ref_alt; } if (control_mode.flag_control_altitude_enabled) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0e7e0ef5d..81f938719 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -169,10 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0f; //[°] --> 47.0 - double lon_current = 0.0f; //[°] --> 8.5 - double alt_current = 0.0f; //[m] above MSL - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - struct pollfd fds_init[2] = { + struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ - bool wait_gps = params.use_gps; + /* wait for initial baro value */ bool wait_baro = true; - hrt_abstime wait_gps_start = 0; - const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix thread_running = true; - while ((wait_gps || wait_baro) && !thread_should_exit) { - int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + while (wait_baro && !thread_should_exit) { + int ret = poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ @@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 /= (float) baro_init_cnt; warnx("init baro: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); - } - } - } - - if (params.use_gps && (fds_init[1].revents & POLLIN)) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - - if (wait_gps && gps.fix_type >= 3) { - hrt_abstime t = hrt_absolute_time(); - - if (wait_gps_start == 0) { - wait_gps_start = t; - - } else if (t - wait_gps_start > wait_gps_delay) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; - - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = t; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.z_valid = true; + local_pos.v_z_valid = true; + local_pos.global_z = true; } } } } } + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + hrt_abstime t_prev = 0; uint16_t accel_updates = 0; @@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 += sonar_corr; warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; @@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (params.use_gps && (fds[5].revents & POLLIN)) { + if (fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + /* initialize reference position if needed */ + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + hrt_abstime t = hrt_absolute_time(); + + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } } gps_updates++; } else { + /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); } } @@ -490,10 +489,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; + bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; + bool flow_valid = false; // TODO implement opt flow + + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be correct in this case */ + bool can_estimate_xy = gps_valid || flow_valid; - if (can_estimate_pos) { + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); @@ -502,12 +505,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - - if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (gps_valid) { + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + } } } @@ -533,43 +537,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; + /* publish local position */ local_pos.timestamp = t; - local_pos.valid = can_estimate_pos; + local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.v_xy_valid = can_estimate_xy; + local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.absolute_alt = local_pos.home_alt - local_pos.z; - local_pos.hdg = att.yaw; - - if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) - && (isfinite(local_pos.y)) - && (isfinite(local_pos.vy)) - && (isfinite(local_pos.z)) - && (isfinite(local_pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - - if (params.use_gps) { - double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - - global_pos.valid = local_pos.valid; - global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = local_pos.home_alt - local_pos.z; - global_pos.relative_alt = -local_pos.z; - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - global_pos.vz = local_pos.vz; - global_pos.hdg = local_pos.hdg; - - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); - } + local_pos.landed = false; // TODO + + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + /* publish global position */ + global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.time_gps_usec = gps.time_gps_usec; + } + /* set valid values even if position is not valid */ + if (local_pos.v_xy_valid) { + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + } + if (local_pos.z_valid) { + global_pos.relative_alt = -local_pos.z; } + if (local_pos.global_z) { + global_pos.alt = local_pos.ref_alt - local_pos.z; + } + if (local_pos.v_z_valid) { + global_pos.vz = local_pos.vz; + } + global_pos.timestamp = t; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 70248b3b7..801e20781 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,6 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); @@ -55,7 +54,6 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); @@ -73,7 +71,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); param_get(h->w_alt_sonar, &(p->w_alt_sonar)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 1e70a3c48..ed6f61e04 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,7 +41,6 @@ #include struct position_estimator_inav_params { - int use_gps; float w_alt_baro; float w_alt_acc; float w_alt_sonar; @@ -56,7 +55,6 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; param_t w_alt_sonar; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 30dc7df9e..7f8648d95 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1042,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; - log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; - log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; - log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 38e2596b2..dd98f65a9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -106,10 +106,9 @@ struct log_LPOS_s { float vx; float vy; float vz; - float hdg; - int32_t home_lat; - int32_t home_lon; - float home_alt; + int32_t ref_lat; + int32_t ref_lon; + float ref_alt; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -260,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 9d3b4468c..26e8f335b 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -54,27 +54,26 @@ */ struct vehicle_local_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - - // TODO Add covariances here - + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool xy_valid; /**< true if x and y are valid */ + bool z_valid; /**< true if z is valid */ + bool v_xy_valid; /**< true if vy and vy are valid */ + bool v_z_valid; /**< true if vz is valid */ + /* Position in local NED frame */ + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< X position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + /* Velocity in NED frame */ + float vx; /**< Ground X Speed (Latitude), m/s in NED */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED */ /* Reference position in GPS / WGS84 frame */ - uint64_t home_timestamp;/**< Time when home position was set */ - int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ - float home_alt; /**< Altitude in meters LOGME */ - float home_hdg; /**< Compass heading in radians -PI..+PI. */ - + bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + uint64_t ref_timestamp; /**< Time when reference position was set */ + int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ + int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6690fb2be..4317e07f2 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -68,8 +68,7 @@ typedef enum { /* navigation state machine */ typedef enum { - NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed - NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization NAVIGATION_STATE_STABILIZE, // attitude stabilization NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization @@ -203,6 +202,7 @@ struct vehicle_status_s bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; + bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ -- cgit v1.2.3 From 3370ceceaf706dda0856888b09c1086e8bf79c8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 08:43:16 +0200 Subject: vehicle_control_mode.flag_armed added, reset integrals in multirotor_att_control when disarmed --- src/modules/commander/commander.cpp | 3 +- .../multirotor_att_control_main.c | 33 +++++++++------------- .../multirotor_attitude_control.c | 9 +++--- .../multirotor_attitude_control.h | 2 +- .../multirotor_rate_control.c | 7 +++-- .../multirotor_rate_control.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 2 ++ 7 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ab7d2e6cf..7d74e0cfe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1235,8 +1235,9 @@ int commander_thread_main(int argc, char *argv[]) } /* publish control mode */ - if (navigation_state_changed) { + if (navigation_state_changed || arming_state_changed) { /* publish new navigation state */ + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic control_mode.counter++; control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 5cad667f6..c057ef364 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -142,16 +142,13 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); /* welcome user */ - printf("[multirotor_att_control] starting\n"); + warnx("starting"); /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; - - /* store if we stopped a yaw movement */ bool reset_yaw_sp = true; /* prepare the handle for the failsafe throttle */ @@ -211,8 +208,7 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /** STEP 1: Define which input is the dominating control input */ + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -220,7 +216,6 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -229,13 +224,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; -// printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { @@ -265,7 +258,7 @@ mc_thread_main(int argc, char *argv[]) * multicopter (throttle = 0) does not make it jump up in the air * if shutting down his remote. */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.thrust = failsafe_throttle; @@ -305,7 +298,6 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } - control_yaw_position = true; } @@ -347,22 +339,25 @@ mc_thread_main(int argc, char *argv[]) } } - /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); + /* run rates controller if needed */ if (control_mode.flag_control_rates_enabled) { /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + bool rates_sp_updated = false; + orb_check(rates_sp_sub, &rates_sp_updated); - if (rates_sp_valid) { + if (rates_sp_updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); } @@ -371,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) rates[0] = att.rollspeed; rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators); + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -391,7 +386,7 @@ mc_thread_main(int argc, char *argv[]) } /* end of poll return value check */ } - printf("[multirotor att control] stopping, disarming motors.\n"); + warnx("stopping, disarming motors"); /* kill all outputs */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4b..12d16f7c7 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index e78f45c47..431a435f7 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -60,6 +60,6 @@ #include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e58d357d5..0a336be47 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 362b5ed86..ca7794c59 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -59,6 +59,6 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, bool reset_integral); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index fe169c6e6..67aeac372 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -64,6 +64,8 @@ struct vehicle_control_mode_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + bool flag_armed; + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ // XXX needs yet to be set by state machine helper -- cgit v1.2.3 From f96bb824d4fc6f6d36ddf24e8879d3025af39d70 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 09:31:33 +0200 Subject: multirotor_pos_control: reset integrals when disarmed --- .../multirotor_pos_control/multirotor_pos_control.c | 16 ++++++++++++---- src/modules/multirotor_pos_control/thrust_pid.c | 5 +++++ src/modules/multirotor_pos_control/thrust_pid.h | 1 + 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 80ce33cda..b2f6b33e3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -301,7 +301,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside + thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -309,8 +309,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - xy_vel_pids[0].integral = 0.0f; - xy_vel_pids[1].integral = 0.0f; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } } else { @@ -439,17 +439,25 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subcrive to velocity setpoint if altitude/position control disabled + // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + bool reset_integral = !control_mode.flag_armed; if (control_mode.flag_control_climb_rate_enabled) { + if (reset_integral) { + thrust_pid_set_integral(&z_vel_pid, params.thr_min); + } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ + if (reset_integral) { + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + } thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 51dcd7df2..b985630ae 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -182,3 +182,8 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa return pid->last_output; } + +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) +{ + pid->integral = i; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 551c032fe..5e169c1ba 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -69,6 +69,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); __END_DECLS -- cgit v1.2.3 From 9610f7a0d7ba7abf7d88c4b3096285e3da68e04d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 09:53:00 +0200 Subject: Fixed merge compile errors --- src/modules/commander/commander.cpp | 103 +++++++++++++++--------------------- 1 file changed, 42 insertions(+), 61 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0f145e1eb..daab7e436 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -418,23 +418,14 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { -<<<<<<< HEAD - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ -======= if (((int)(cmd->param1)) == 1) { /* reboot */ - up_systemreset(); + systemreset(false); } else if (((int)(cmd->param1)) == 3) { /* reboot to bootloader */ - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 + systemreset(true); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -768,13 +759,7 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { -<<<<<<< HEAD - hrt_abstime t = hrt_absolute_time(); - status_changed = false; - -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* update parameters */ orb_check(param_changed_sub, &updated); @@ -882,17 +867,11 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); -<<<<<<< HEAD - - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { -======= - warnx("bat v: %2.2f", battery.voltage_v); + // warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -1010,39 +989,39 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ -<<<<<<< HEAD - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - // bool global_pos_valid = status.condition_global_position_valid; - // bool local_pos_valid = status.condition_local_position_valid; - // bool airspeed_valid = status.condition_airspeed_valid; +// <<<<<<< HEAD +// /* store current state to reason later about a state change */ +// // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; +// // bool global_pos_valid = status.condition_global_position_valid; +// // bool local_pos_valid = status.condition_local_position_valid; +// // bool airspeed_valid = status.condition_airspeed_valid; - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; +// /* check for global or local position updates, set a timeout of 2s */ +// if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { +// status.condition_global_position_valid = true; - } else { - status.condition_global_position_valid = false; - } +// } else { +// status.condition_global_position_valid = false; +// } - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; +// if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { +// status.condition_local_position_valid = true; - } else { - status.condition_local_position_valid = false; - } +// } else { +// status.condition_local_position_valid = false; +// } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; +// /* Check for valid airspeed/differential pressure measurements */ +// if (t - last_diff_pres_time < 2000000 && t > 2000000) { +// status.condition_airspeed_valid = true; - } else { - status.condition_airspeed_valid = false; - } +// } else { +// status.condition_airspeed_valid = false; +// } -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 +// ======= +// >>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 orb_check(gps_sub, &updated); if (updated) { @@ -1362,9 +1341,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1433,6 +1412,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { @@ -1512,18 +1503,8 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* not ready to arm, blink at 10Hz */ } -<<<<<<< HEAD rgbled_set_pattern(&pattern); } -======= - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 6) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { -- cgit v1.2.3 From 1ae5a6ac1d3da98e5612b77ff28afa86669c287f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:02:15 +0200 Subject: Minimized nshterm flags, fixed some pretty stupid non-standard coding in top, now behaving with two shell instances as expected --- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/top/top.c | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 2341068a2..41d108ffc 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -91,7 +91,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05e..1ca3fc928 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) 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); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } -- cgit v1.2.3 From a9743f04be59aee7bb96a5bb99ae8d8155eb19de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:09:06 +0200 Subject: Fixed status changed flag --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daab7e436..98aab8788 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1317,6 +1317,8 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { + status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1446,8 +1448,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - warnx("changed"); - int i; rgbled_pattern_t pattern; memset(&pattern, 0, sizeof(pattern)); -- cgit v1.2.3 From cdd09333f946e746527c8e9af36e08dc3a29a975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:02:15 +0200 Subject: Minimized nshterm flags, fixed some pretty stupid non-standard coding in top, now behaving with two shell instances as expected --- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/top/top.c | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 2341068a2..41d108ffc 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -91,7 +91,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05e..1ca3fc928 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) 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); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } -- cgit v1.2.3 From 871b4c19bc65bf923887e0bd32e1889db1c71aca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:19:51 +0200 Subject: Added stop command to RGB led --- src/drivers/rgbled/rgbled.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 467379a77..05f079ede 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -443,7 +443,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb'"); + warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -534,7 +534,8 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "stop") || !strcmp(verb, "off")) { + /* although technically it doesn't stop, this is the excepted syntax */ fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); -- cgit v1.2.3 From 12df5dd2699f163bc5551b2be611665fc58fb001 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:32:56 +0200 Subject: Corrected orientation of external mag --- src/drivers/hmc5883/hmc5883.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9e9c067d5..692f890bd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -860,12 +860,13 @@ HMC5883::collect() _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* the standard external mag seems to be rolled 180deg, therefore y and z inverted */ - _reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, + * therefore switch and invert x and y */ + _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; + _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif -- cgit v1.2.3 From 00c9b8f87f40ffca47ade881087a50b6162fd185 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:34:12 +0200 Subject: Start the hmc5883 before the lsm303d so that an external mag is used --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b..762e80e1f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -18,10 +18,12 @@ fi ms5611 start adc start +# mag might be external +hmc5883 start + if mpu6000 start then - echo "using MPU6000 and HMC5883L" - hmc5883 start + echo "using MPU6000" set BOARD fmuv1 else echo "using L3GD20 and LSM303D" -- cgit v1.2.3 From a95e48c0b25c75548e923657247cdf268e654097 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:51:22 +0200 Subject: Don't stop the startup script if no external HMC5883 is connected --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 762e80e1f..61bb09728 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -19,7 +19,10 @@ ms5611 start adc start # mag might be external -hmc5883 start +if hmc5883 start +then + echo "using HMC5883" +fi if mpu6000 start then -- cgit v1.2.3 From f4b5a17a7b6385869933cd195afd674fa532e735 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:35:07 +0200 Subject: Improved sensor startup and error checking --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++++ src/modules/sensors/sensors.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b..4cfd59d54 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,10 @@ then hmc5883 start set BOARD fmuv1 else + if hmc5883 start + then + echo "Using external mag" + fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a6204c9fa..6e57a79a8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -953,7 +953,7 @@ Sensors::baro_init() if (fd < 0) { warn("%s", BARO_DEVICE_PATH); - warnx("No barometer found, ignoring"); + errx(1, "FATAL: No barometer found"); } /* set the driver to poll at 150Hz */ diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index d1dd85d47..e7d9ce85f 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -263,7 +263,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 150; i++) + for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); -- cgit v1.2.3 From 7bd952c7acf7bc1c588a123d395930525be1349c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:38:03 +0200 Subject: Prevented double HMC start after merge --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 02b70e7da..61bb09728 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -29,10 +29,6 @@ then echo "using MPU6000" set BOARD fmuv1 else - if hmc5883 start - then - echo "Using external mag" - fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start -- cgit v1.2.3 From 449dc78ae69e888d986185f120aa8c6549ef5c2b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 18:33:04 +0200 Subject: commander, multirotor_pos_control, sdlog2: bugfixes --- src/modules/commander/commander.cpp | 17 +- .../multirotor_pos_control.c | 249 ++++++++++++++------- .../multirotor_pos_control_params.c | 7 +- .../multirotor_pos_control_params.h | 11 +- .../position_estimator_inav_main.c | 7 + src/modules/sdlog2/sdlog2_messages.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 6 +- 7 files changed, 197 insertions(+), 102 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d74e0cfe..50acec7e0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -353,16 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (armed->armed) { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } else { + /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b2f6b33e3..0d5a537ea 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,14 +211,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - bool reset_sp_alt = true; - bool reset_sp_pos = true; + bool global_pos_sp_reproject = false; + bool global_pos_sp_valid = false; + bool local_pos_sp_valid = false; + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_integral = true; + hrt_abstime t_prev = 0; /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float home_alt = 0.0f; - hrt_abstime home_alt_t = 0; + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -242,11 +251,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; - bool global_pos_sp_updated = false; while (!thread_should_exit) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz */ if (++paramcheck_counter >= 50) { paramcheck_counter = 0; @@ -260,11 +266,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); /* use integral_limit_out = tilt_max / 2 */ float i_limit; + if (params.xy_vel_i == 0.0) { i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { i_limit = 1.0f; // not used really } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } @@ -273,9 +282,20 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* only check global position setpoint updates but not read */ - if (!global_pos_sp_updated) { - orb_check(global_pos_sp_sub, &global_pos_sp_updated); + bool updated; + + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(global_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + global_pos_sp_valid = true; + global_pos_sp_reproject = true; } hrt_abstime t = hrt_absolute_time(); @@ -288,6 +308,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) dt = 0.0f; } + if (control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = control_mode.flag_armed; + t_prev = t; if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { @@ -296,77 +326,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (control_mode.flag_control_manual_enabled) { - /* manual mode, reset setpoints if needed */ - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside - mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - } - - if (control_mode.flag_control_position_enabled && reset_sp_pos) { - reset_sp_pos = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - } - } else { - /* non-manual mode, project global setpoints to local frame */ - //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.ref_timestamp != local_ref_timestamp) { - local_ref_timestamp = local_pos.ref_timestamp; - /* init local projection using local position home */ - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); - mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_updated) { - global_pos_sp_updated = false; - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - } - } - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.ref_timestamp != home_alt_t) { - if (home_alt_t != 0) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - ref_alt; } - home_alt_t = local_pos.ref_timestamp; - home_alt = local_pos.ref_alt; + ref_alt_t = local_pos.ref_timestamp; + ref_alt = local_pos.ref_alt; + // TODO also correct XY setpoint } + /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with manual controls */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + } + + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { @@ -383,7 +369,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - /* move position setpoint with manual controls */ + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); @@ -410,12 +405,68 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + + /* local position setpoint is valid and can be used for loiter after position controlled mode */ + local_pos_sp_valid = control_mode.flag_control_position_enabled; + + /* force reprojection of global setpoint after manual mode */ + global_pos_sp_reproject = true; + + } else { + /* non-manual mode, use global setpoint */ + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + } + + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + } else { + reset_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -426,13 +477,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { - reset_sp_pos = true; + reset_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -444,20 +496,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - bool reset_integral = !control_mode.flag_armed; + if (control_mode.flag_control_climb_rate_enabled) { - if (reset_integral) { - thrust_pid_set_integral(&z_vel_pid, params.thr_min); + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; + + if (reset_int_z_manual) { + i = manual.throttle; + + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; + } + } + + thrust_pid_set_integral(&z_vel_pid, -i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); } + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; + + } else { + /* reset thrust integral when altitude control enabled */ + reset_int_z = true; } + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ - if (reset_integral) { + if (reset_int_xy) { + reset_int_xy = false; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); @@ -471,11 +547,15 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (tilt > params.tilt_max) { tilt = params.tilt_max; } + /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); + + } else { + reset_int_xy = true; } att_sp.timestamp = hrt_absolute_time(); @@ -483,14 +563,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else { - reset_sp_alt = true; - reset_sp_pos = true; + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + global_pos_sp_reproject = true; } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; + /* run at approximately 50 Hz */ usleep(20000); - } warnx("stopped"); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 1094fd23b..0b09c9ea7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +34,7 @@ /* * @file multirotor_pos_control_params.c - * + * * Parameters for multirotor_pos_control */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 14a3de2e5..3ec85a364 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +33,9 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81f938719..c0cfac880 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -508,6 +508,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -554,6 +555,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* publish global position */ global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); @@ -561,21 +563,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.time_gps_usec = gps.time_gps_usec; } + /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); } + if (local_pos.z_valid) { global_pos.relative_alt = -local_pos.z; } + if (local_pos.global_z) { global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index dd98f65a9..d99892fe2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -259,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 67aeac372..4f4db5dbc 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,7 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; @@ -73,11 +73,9 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // XXX shouldn't be necessairy - //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ -- cgit v1.2.3 From de124619b6f64aafc10f1cdebef986a9e193b74b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 20:26:00 +0200 Subject: RC mode switching diagram updated --- Documentation/rc_mode_switch.odg | Bin 14872 -> 22232 bytes Documentation/rc_mode_switch.pdf | Bin 16097 -> 28728 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index a8a6f93f3..29d738c39 100644 Binary files a/Documentation/rc_mode_switch.odg and b/Documentation/rc_mode_switch.odg differ diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index 3141eed7f..856dd55c5 100644 Binary files a/Documentation/rc_mode_switch.pdf and b/Documentation/rc_mode_switch.pdf differ -- cgit v1.2.3 From 95260d453592903bfcd3dba9379db033738d5b89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Aug 2013 09:36:26 +0200 Subject: Fixed NSH terminal init --- ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- src/systemcmds/nshterm/nshterm.c | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index bb78b6a65..7f0409519 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -61,11 +61,7 @@ then # # Start terminal # -if sercon -then - echo "USB connected" - nshterm /dev/ttyACM0 & -fi +sercon # # Start the ORB (first app to start) @@ -164,5 +160,8 @@ then sh /etc/init.d/31_io_phantom fi +# Try to get an USB console +nshterm /dev/ttyACM0 & + # End of autostart fi diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 41d108ffc..458bb2259 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,7 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 5) { + while (retries < 50) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); -- cgit v1.2.3 From 6dcad01d03251093da2ee4ee0026672b224aae72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 10:35:23 +0200 Subject: Fixed accel self test --- src/drivers/mpu6000/mpu6000.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 9d498bbd6..bfc74c73e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -689,6 +689,8 @@ MPU6000::accel_self_test() return 1; if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) return 1; + + return 0; } int -- cgit v1.2.3 From db950f74893a108302a167729a91765269981e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 Aug 2013 12:17:15 +0200 Subject: position_estimator_inav: "landed" detector implemented, bugfixes --- src/modules/commander/commander.cpp | 11 ++ .../multirotor_pos_control.c | 4 + .../multirotor_pos_control_params.c | 4 +- .../position_estimator_inav_main.c | 128 ++++++++++++++------- .../position_estimator_inav_params.c | 11 +- .../position_estimator_inav_params.h | 6 + 6 files changed, 118 insertions(+), 46 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50acec7e0..04e6dd2cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -820,6 +820,17 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + if (status.condition_local_altitude_valid) { + if (status.condition_landed != local_position.landed) { + status.condition_landed = local_position.landed; + status_changed = true; + if (status.condition_landed) { + mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + } + } + } /* update battery status */ orb_check(battery_sub, &updated); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0d5a537ea..a6ebeeacb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -459,6 +459,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint as projection of global position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + + /* reset setpoints after non-manual modes */ + reset_sp_xy = true; + reset_sp_z = true; } /* run position & altitude controllers, calculate velocity setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 0b09c9ea7..9c1ef2edb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -41,8 +41,8 @@ #include "multirotor_pos_control_params.h" /* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c0cfac880..3466841c4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -52,10 +52,11 @@ #include #include #include -#include #include +#include +#include +#include #include -#include #include #include #include @@ -169,14 +170,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ + float alt_avg = 0.0f; + bool landed = true; + hrt_abstime landed_time = 0; uint32_t accel_counter = 0; uint32_t baro_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - /* make sure that baroINITdone = false */ + struct actuator_controls_effective_s actuator; + memset(&actuator, 0, sizeof(actuator)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; @@ -192,7 +197,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); @@ -294,9 +300,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime sonar_time = 0; /* main loop */ - struct pollfd fds[6] = { + struct pollfd fds[7] = { { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = actuator_sub, .events = POLLIN }, + { .fd = armed_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = optical_flow_sub, .events = POLLIN }, @@ -308,7 +315,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -328,20 +335,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) parameters_update(&pos_inav_param_handles, ¶ms); } - /* vehicle status */ + /* actuator */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); } - /* vehicle attitude */ + /* armed */ if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + } + + /* vehicle attitude */ + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } /* sensor combined */ - if (fds[3].revents & POLLIN) { + if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { @@ -378,7 +389,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* optical flow */ - if (fds[4].revents & POLLIN) { + if (fds[5].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { @@ -415,33 +426,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (fds[5].revents & POLLIN) { - /* vehicle GPS position */ + /* vehicle GPS position */ + if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ - if (ref_xy_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; - - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; - } - - } else { - hrt_abstime t = hrt_absolute_time(); - + if (!ref_xy_inited) { if (ref_xy_init_start == 0) { ref_xy_init_start = t; @@ -462,12 +453,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - gps_updates++; + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + } } else { /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); + ref_xy_init_start = 0; } + + gps_updates++; } } @@ -516,9 +527,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* detect land */ + alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp = z_est[0] - alt_avg; + alt_disp = alt_disp * alt_disp; + float land_disp2 = params.land_disp * params.land_disp; + /* get actual thrust output */ + float thrust = armed.armed ? actuator.control_effective[3] : 0.0f; + + if (landed) { + if (alt_disp > land_disp2 && thrust > params.land_thr) { + landed = false; + landed_time = 0; + } + + } else { + if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (landed_time == 0) { + landed_time = t; // land detected first time + + } else { + if (t > landed_time + params.land_t * 1000000.0f) { + landed = true; + landed_time = 0; + } + } + + } else { + landed_time = 0; + } + } + if (verbose_mode) { /* print updates rate */ - if (t - updates_counter_start > updates_counter_len) { + if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", @@ -536,7 +578,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (t - pub_last > pub_interval) { + if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ local_pos.timestamp = t; @@ -549,7 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = false; // TODO + local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 801e20781..4f9ddd009 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); @@ -51,6 +51,9 @@ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); +PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -65,6 +68,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->flow_k = param_find("INAV_FLOW_K"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); + h->land_t = param_find("INAV_LAND_T"); + h->land_disp = param_find("INAV_LAND_DISP"); + h->land_thr = param_find("INAV_LAND_THR"); return OK; } @@ -82,6 +88,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->flow_k, &(p->flow_k)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->land_t, &(p->land_t)); + param_get(h->land_disp, &(p->land_disp)); + param_get(h->land_thr, &(p->land_thr)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index ed6f61e04..61570aea7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -52,6 +52,9 @@ struct position_estimator_inav_params { float flow_k; float sonar_filt; float sonar_err; + float land_t; + float land_disp; + float land_thr; }; struct position_estimator_inav_param_handles { @@ -66,6 +69,9 @@ struct position_estimator_inav_param_handles { param_t flow_k; param_t sonar_filt; param_t sonar_err; + param_t land_t; + param_t land_disp; + param_t land_thr; }; /** -- cgit v1.2.3 From 75a6e095676a7951c7077f757b7a10ea1399729f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:20:34 +0200 Subject: Ignore files for ctags (used with SublimeText3) --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3fdc1bf2a..76f45281c 100644 --- a/.gitignore +++ b/.gitignore @@ -27,4 +27,6 @@ mavlink/include/mavlink/v0.9/ /NuttX /Documentation/doxy.log /Documentation/html/ -/Documentation/doxygen*objdb*tmp \ No newline at end of file +/Documentation/doxygen*objdb*tmp +.tags +.tags_sorted_by_file \ No newline at end of file -- cgit v1.2.3 From d2d59aa39278de9461ff72061cbd8a89d7e81f4b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:04:57 +0200 Subject: Handle the config command line arguments a bit more intuitive --- src/systemcmds/config/config.c | 146 +++++++++++++++++------------------------ 1 file changed, 59 insertions(+), 87 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 5a02fd620..766598ddd 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -2,6 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +36,7 @@ /** * @file config.c * @author Lorenz Meier + * @author Julian Oes * * config tool. */ @@ -69,27 +71,15 @@ config_main(int argc, char *argv[]) { if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { - if (argc >= 3) { - do_gyro(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_gyro(argc - 2, argv + 2); } if (!strcmp(argv[1], "accel")) { - if (argc >= 3) { - do_accel(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_accel(argc - 2, argv + 2); } if (!strcmp(argv[1], "mag")) { - if (argc >= 3) { - do_mag(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_mag(argc - 2, argv + 2); } } @@ -109,44 +99,36 @@ do_gyro(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the gyro internal sampling rate up to at least i Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { - - /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, i); - } + /* set the range to i dps */ + ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); - } else if (argc > 0) { + } else if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, GYROIOCSELFTEST, 0); - if(!strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret) { - warnx("gyro self test FAILED! Check calibration:"); - struct gyro_scale scale; - ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); - } else { - warnx("gyro calibration and self test OK"); - } + if (ret) { + warnx("gyro self test FAILED! Check calibration:"); + struct gyro_scale scale; + ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("gyro calibration and self test OK"); } } else { - warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); } int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); @@ -174,29 +156,26 @@ do_mag(int argc, char *argv[]) } else { - if (argc > 0) { + if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, MAGIOCSELFTEST, 0); - if (!strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); - - if (ret) { - warnx("mag self test FAILED! Check calibration."); - struct mag_scale scale; - ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); - } else { - warnx("mag calibration and self test OK"); - } + if (ret) { + warnx("mag self test FAILED! Check calibration:"); + struct mag_scale scale; + ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("mag calibration and self test OK"); } } else { - warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t"); + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1;//ioctl(fd, MAGIOCGRANGE, 0); + int range = -1; //ioctl(fd, MAGIOCGRANGE, 0); warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); @@ -219,43 +198,36 @@ do_accel(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the accel internal sampling rate up to at least i Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + /* set the range to i m/s^2 */ + ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); - /* set the range to i dps */ - ioctl(fd, ACCELIOCSRANGE, i); - } - } else if (argc > 0) { - - if (!strcmp(argv[0], "check")) { - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret) { - warnx("accel self test FAILED! Check calibration."); - struct accel_scale scale; - ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); - warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); - } else { - warnx("accel calibration and self test OK"); - } + } else if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret) { + warnx("accel self test FAILED! Check calibration:"); + struct accel_scale scale; + ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("accel calibration and self test OK"); } } else { - warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t"); + errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); -- cgit v1.2.3 From 307c9e52c775de2ce09ff4abf0bc1fb5db6dd41e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 19:50:58 +0200 Subject: Sorry, finally got the axes of the external mag right --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 692f890bd..44304fc22 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -861,10 +861,10 @@ HMC5883::collect() } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch and invert x and y */ + * therefore switch x and y and invert y */ _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD -- cgit v1.2.3 From f5c92314f16fde650ee6f2f4fa20b7c2680a4b00 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 20:02:06 +0200 Subject: Improved LSM303D driver, plus some fixes to the HMC5883 --- src/drivers/drv_mag.h | 25 ++- src/drivers/hmc5883/hmc5883.cpp | 5 + src/drivers/lsm303d/lsm303d.cpp | 482 +++++++++++++++++++++++----------------- src/modules/sensors/sensors.cpp | 25 ++- src/systemcmds/config/config.c | 71 ++++-- 5 files changed, 379 insertions(+), 229 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e95034e8e..3de5625fd 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag); /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) +/** return the mag internal sample rate in Hz */ +#define MAGIOCGSAMPLERATE _MAGIOC(1) + /** set the mag internal lowpass filter to no lower than (arg) Hz */ -#define MAGIOCSLOWPASS _MAGIOC(1) +#define MAGIOCSLOWPASS _MAGIOC(2) + +/** return the mag internal lowpass filter in Hz */ +#define MAGIOCGLOWPASS _MAGIOC(3) /** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(2) +#define MAGIOCSSCALE _MAGIOC(4) /** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(3) +#define MAGIOCGSCALE _MAGIOC(5) /** set the measurement range to handle (at least) arg Gauss */ -#define MAGIOCSRANGE _MAGIOC(4) +#define MAGIOCSRANGE _MAGIOC(6) + +/** return the current mag measurement range in Gauss */ +#define MAGIOCGRANGE _MAGIOC(7) /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(5) +#define MAGIOCCALIBRATE _MAGIOC(8) /** excite strap */ -#define MAGIOCEXSTRAP _MAGIOC(6) +#define MAGIOCEXSTRAP _MAGIOC(9) /** perform self test and report status */ -#define MAGIOCSELFTEST _MAGIOC(7) +#define MAGIOCSELFTEST _MAGIOC(10) /** determine if mag is external or onboard */ -#define MAGIOCGEXTERNAL _MAGIOC(8) +#define MAGIOCGEXTERNAL _MAGIOC(11) #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 44304fc22..1afc16a9a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -637,13 +637,18 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: /* not supported, always 1 sample per poll */ return -EINVAL; case MAGIOCSRANGE: return set_range(arg); + case MAGIOCGRANGE: + return _range_ga; + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e6ce64b8..8d6dc0672 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -180,56 +180,62 @@ public: LSM303D(int bus, const char* path, spi_dev_e device); virtual ~LSM303D(); - virtual int init(); + virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); friend class LSM303D_mag; virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); private: - LSM303D_mag *_mag; + LSM303D_mag *_mag; struct hrt_call _accel_call; struct hrt_call _mag_call; - unsigned _call_accel_interval; - unsigned _call_mag_interval; + unsigned _call_accel_interval; + unsigned _call_mag_interval; - unsigned _num_accel_reports; + unsigned _num_accel_reports; volatile unsigned _next_accel_report; volatile unsigned _oldest_accel_report; struct accel_report *_accel_reports; - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _current_samplerate; - - unsigned _num_mag_reports; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; struct mag_report *_mag_reports; + struct accel_scale _accel_scale; + unsigned _accel_range_g; + float _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_filter_bandwith; + struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; orb_advert_t _mag_topic; + unsigned _accel_read; + unsigned _mag_read; + perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; @@ -240,12 +246,19 @@ private: /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -256,24 +269,38 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void measure_trampoline(void *arg); + static void measure_trampoline(void *arg); /** * Static trampoline for the mag because it runs at a lower rate * * @param arg Instance pointer for the driver that is polling. */ - static void mag_measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void mag_measure(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Mag self test + * + * @return 0 on success, 1 on failure + */ + int mag_self_test(); /** * Read a register from the LSM303D @@ -281,7 +308,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the LSM303D @@ -289,7 +316,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the LSM303D @@ -300,7 +327,7 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** * Set the LSM303D accel measurement range. @@ -309,7 +336,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int accel_set_range(unsigned max_g); /** * Set the LSM303D mag measurement range. @@ -318,7 +345,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int mag_set_range(unsigned max_g); + int mag_set_range(unsigned max_g); /** * Set the LSM303D accel anti-alias filter. @@ -327,15 +354,7 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned bandwith); - - /** - * Get the LSM303D accel anti-alias filter. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * @return OK if the value was read and supported, ERROR otherwise. - */ - int get_antialias_filter_bandwidth(unsigned &bandwidth); + int accel_set_antialias_filter_bandwidth(unsigned bandwith); /** * Set the LSM303D internal accel sampling frequency. @@ -345,7 +364,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int accel_set_samplerate(unsigned frequency); /** * Set the LSM303D internal mag sampling frequency. @@ -355,7 +374,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int mag_set_samplerate(unsigned frequency); + int mag_set_samplerate(unsigned frequency); }; /** @@ -396,16 +415,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_accel_report(0), _oldest_accel_report(0), _accel_reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _current_samplerate(0), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), + _accel_range_g(8), + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(800), + _accel_filter_bandwith(50), + _mag_range_ga(2), _mag_range_scale(0.0f), - _mag_range_ga(0.0f), + _mag_samplerate(100), + _accel_topic(-1), + _mag_topic(-1), + _accel_read(0), + _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_filter_x(800, 30), @@ -416,18 +441,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _debug_enabled = true; // default scale factors - _accel_scale.x_offset = 0; + _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; + _accel_scale.y_offset = 0.0f; _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; + _accel_scale.z_offset = 0.0f; _accel_scale.z_scale = 1.0f; - _mag_scale.x_offset = 0; + _mag_scale.x_offset = 0.0f; _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; + _mag_scale.y_offset = 0.0f; _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; + _mag_scale.z_offset = 0.0f; _mag_scale.z_scale = 1.0f; } @@ -478,27 +503,12 @@ LSM303D::init() if (_mag_reports == nullptr) goto out; + reset(); + /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - /* enable accel, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); - - /* enable mag, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - - /* XXX should we enable FIFO? */ - - set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ - set_samplerate(400); /* max sample rate */ - - mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ - mag_set_samplerate(100); - - /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -511,6 +521,24 @@ out: return ret; } +void +LSM303D::reset() +{ + /* enable accel*/ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + + /* enable mag */ + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + + accel_set_range(_accel_range_g); + accel_set_samplerate(_accel_samplerate); + accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + + mag_set_range(_mag_range_ga); + mag_set_samplerate(_mag_samplerate); +} + int LSM303D::probe() { @@ -612,64 +640,60 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_accel_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - /* XXX for vibration tests with 800 Hz */ + /* Use 800Hz as it is filtered in the driver as well*/ return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; - /* adjust sample rate of sensor */ - set_samplerate(arg); + /* adjust filters */ + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_call_accel_interval == 0) @@ -678,66 +702,78 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* account for sentinel in the ring */ + arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; - if (nullptr == buf) - return -ENOMEM; + if (nullptr == buf) + return -ENOMEM; - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _num_accel_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; case ACCELIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_accel_interval; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + return OK; + } case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); - - case ACCELIOCSSCALE: - { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; - } + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; } + } + + case ACCELIOCSRANGE: + return accel_set_range(arg); + + case ACCELIOCGRANGE: + return _accel_range_g; case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; + case ACCELIOCSELFTEST: + return accel_self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -768,7 +804,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* 50 Hz is max for mag */ + /* 100 Hz is max for mag */ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ @@ -783,9 +819,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; - /* adjust sample rate of sensor */ - mag_set_samplerate(arg); - /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; @@ -833,17 +866,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return _num_mag_reports - 1; case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + reset(); + return OK; case MAGIOCSSAMPLERATE: -// case MAGIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; case MAGIOCSLOWPASS: -// case MAGIOCGLOWPASS: - /* XXX not implemented */ -// _set_dlpf_filter((uint16_t)arg); + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: @@ -857,17 +891,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case MAGIOCSRANGE: -// case MAGIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _mag_range_scale = xx - // _mag_range_ga = xx - return -EINVAL; + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; case MAGIOCSELFTEST: - /* XXX not implemented */ -// return self_test(); - return -EINVAL; + return mag_self_test(); case MAGIOCGEXTERNAL: /* no external mag board yet */ @@ -879,6 +909,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +LSM303D::accel_self_test() +{ + if (_accel_read == 0) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; + + return 0; +} + +int +LSM303D::mag_self_test() +{ + if (_mag_read == 0) + return 1; + + /** + * inspect mag offsets + * don't check mag scale because it seems this is calibrated on chip + */ + if (fabsf(_mag_scale.x_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.y_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.z_offset) < 0.000001f) + return 1; + + return 0; +} + uint8_t LSM303D::read_reg(unsigned reg) { @@ -914,38 +991,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) } int -LSM303D::set_range(unsigned max_g) +LSM303D::accel_set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range_g = 0.0f; float new_scale_g_digit = 0.0f; if (max_g == 0) max_g = 16; if (max_g <= 2) { - new_range_g = 2.0f; + _accel_range_g = 2; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range_g = 4.0f; + _accel_range_g = 4; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range_g = 6.0f; + _accel_range_g = 6; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range_g = 8.0f; + _accel_range_g = 8; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range_g = 16.0f; + _accel_range_g = 16; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -953,7 +1029,7 @@ LSM303D::set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -966,29 +1042,28 @@ LSM303D::mag_set_range(unsigned max_ga) { uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; - float new_range = 0.0f; float new_scale_ga_digit = 0.0f; if (max_ga == 0) max_ga = 12; if (max_ga <= 2) { - new_range = 2.0f; + _mag_range_ga = 2; setbits |= REG6_FULL_SCALE_2GA_M; new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { - new_range = 4.0f; + _mag_range_ga = 4; setbits |= REG6_FULL_SCALE_4GA_M; new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { - new_range = 8.0f; + _mag_range_ga = 8; setbits |= REG6_FULL_SCALE_8GA_M; new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { - new_range = 12.0f; + _mag_range_ga = 12; setbits |= REG6_FULL_SCALE_12GA_M; new_scale_ga_digit = 0.479e-3f; @@ -996,7 +1071,6 @@ LSM303D::mag_set_range(unsigned max_ga) return -EINVAL; } - _mag_range_ga = new_range; _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1005,7 +1079,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1015,15 +1089,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_filter_bandwith = 773; } else { return -EINVAL; @@ -1035,26 +1113,7 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) } int -LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) -{ - uint8_t readbits = read_reg(ADDR_CTRL_REG2); - - if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) - bandwidth = 50; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) - bandwidth = 194; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) - bandwidth = 362; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) - bandwidth = 773; - else - return ERROR; - - return OK; -} - -int -LSM303D::set_samplerate(unsigned frequency) +LSM303D::accel_set_samplerate(unsigned frequency) { uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; @@ -1064,23 +1123,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; - _current_samplerate = 100; + _accel_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; - _current_samplerate = 200; + _accel_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; - _current_samplerate = 400; + _accel_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; - _current_samplerate = 800; + _accel_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; - _current_samplerate = 1600; + _accel_samplerate = 1600; } else { return -EINVAL; @@ -1102,13 +1161,15 @@ LSM303D::mag_set_samplerate(unsigned frequency) if (frequency <= 25) { setbits |= REG5_RATE_25HZ_M; + _mag_samplerate = 25; } else if (frequency <= 50) { setbits |= REG5_RATE_50HZ_M; + _mag_samplerate = 50; } else if (frequency <= 100) { setbits |= REG5_RATE_100HZ_M; - + _mag_samplerate = 100; } else { return -EINVAL; @@ -1229,6 +1290,8 @@ LSM303D::measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + _accel_read++; + /* stop the perf counter */ perf_end(_accel_sample_perf); } @@ -1281,7 +1344,7 @@ LSM303D::mag_measure() mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report->scaling = _mag_range_scale; - mag_report->range_ga = _mag_range_ga; + mag_report->range_ga = (float)_mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); @@ -1297,6 +1360,8 @@ LSM303D::mag_measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + _mag_read++; + /* stop the perf counter */ perf_end(_mag_sample_perf); } @@ -1304,6 +1369,8 @@ LSM303D::mag_measure() void LSM303D::print_info() { + printf("accel reads: %u\n", _accel_read); + printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); @@ -1466,7 +1533,7 @@ test() /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) errx(1, "failed to get if mag is onboard or external"); - warnx("device active: %s", ret ? "external" : "onboard"); + warnx("mag device active: %s", ret ? "external" : "onboard"); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); @@ -1484,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ -// reset(); + reset(); errx(0, "PASS"); } @@ -1503,7 +1570,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel driver poll rate reset failed"); + + int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) { + warnx("could not open to mag " MAG_DEVICE_PATH); + } else { + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "mag driver poll rate reset failed"); + } exit(0); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 198da9f0a..7ea1ae0f3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -936,6 +936,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -944,13 +945,27 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); +#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } +#endif + - int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 766598ddd..262a52d20 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -90,6 +90,7 @@ static void do_gyro(int argc, char *argv[]) { int fd; + int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -102,20 +103,29 @@ do_gyro(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { warnx("gyro self test FAILED! Check calibration:"); @@ -147,6 +157,7 @@ static void do_mag(int argc, char *argv[]) { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -156,8 +167,32 @@ do_mag(int argc, char *argv[]) } else { - if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); + if (argc == 2 && !strcmp(argv[0], "sampling")) { + + /* set the mag internal sampling rate up to at least i Hz */ + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i G */ + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { warnx("mag self test FAILED! Check calibration:"); @@ -173,9 +208,9 @@ do_mag(int argc, char *argv[]) errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1; //ioctl(fd, MAGIOCGRANGE, 0); + int range = ioctl(fd, MAGIOCGRANGE, 0); warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); @@ -189,6 +224,7 @@ static void do_accel(int argc, char *argv[]) { int fd; + int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -201,20 +237,29 @@ do_accel(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the range to i m/s^2 */ - ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + /* set the range to i G */ + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { warnx("accel self test FAILED! Check calibration:"); -- cgit v1.2.3 From 408b29ba618e1c2c7375d6acd488c223d796186f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 08:40:51 +0200 Subject: Don't store m/s^2 and G at the same time --- src/drivers/lsm303d/lsm303d.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8d6dc0672..2b7769992 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -220,7 +220,6 @@ private: struct accel_scale _accel_scale; unsigned _accel_range_g; - float _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_filter_bandwith; @@ -420,7 +419,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_g(8), - _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(800), _accel_filter_bandwith(50), @@ -1029,7 +1027,6 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1275,7 +1272,7 @@ LSM303D::measure() accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_m_s2; + accel_report->range_m_s2 = _accel_range_g * 9.80665f; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); -- cgit v1.2.3 From 658276e1cc5f4639fb09ff41a20b422e6ce33fe3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:23:21 +0200 Subject: Add reset and samplerate ioctl to HMC5883 driver --- src/drivers/hmc5883/hmc5883.cpp | 107 ++++++++++++++++++++++------------------ 1 file changed, 60 insertions(+), 47 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1afc16a9a..cb708db4a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -191,6 +191,11 @@ private: */ void stop(); + /** + * Reset the device + */ + int reset(); + /** * Perform the on-sensor scale calibration routine. * @@ -365,6 +370,9 @@ HMC5883::init() if (I2C::init() != OK) goto out; + /* reset the device configuration */ + reset(); + /* allocate basic report buffers */ _num_reports = 2; _reports = new struct mag_report[_num_reports]; @@ -381,9 +389,6 @@ HMC5883::init() if (_mag_topic < 0) debug("failed to create sensor_mag object"); - /* set range */ - set_range(_range_ga); - ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -542,62 +547,61 @@ int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) + return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) @@ -633,13 +637,15 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return _num_reports - 1; case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + return reset(); case MAGIOCSSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + case MAGIOCGSAMPLERATE: - /* not supported, always 1 sample per poll */ - return -EINVAL; + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCGPOLLRATE, 0); case MAGIOCSRANGE: return set_range(arg); @@ -702,6 +708,13 @@ HMC5883::stop() work_cancel(HPWORK, &_work); } +int +HMC5883::reset() +{ + /* set range */ + return set_range(_range_ga); +} + void HMC5883::cycle_trampoline(void *arg) { -- cgit v1.2.3 From 9762cf86a081f44e7f7ea48f160d556003bf5be9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:52:21 +0200 Subject: Forgot to comment mag init in sensors.cpp back back in --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ea1ae0f3..dda558b4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -947,7 +947,7 @@ Sensors::mag_init() /* try different mag sampling rates */ -#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ @@ -962,7 +962,7 @@ Sensors::mag_init() errx(1, "FATAL: mag sampling rate could not be set"); } } -#endif + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); -- cgit v1.2.3 From 3875df2fe07d33d00331efd02394201d684655c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 10:44:47 +0200 Subject: Workaround to get the HMC5883 default rate right --- src/drivers/hmc5883/hmc5883.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cb708db4a..d77f03bb7 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,8 +77,8 @@ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -/* Max measurement rate is 160Hz */ -#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ +/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ +#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ #define ADDR_CONF_A 0x00 #define ADDR_CONF_B 0x01 @@ -607,7 +607,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return 1000000/TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -645,7 +645,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ - return ioctl(filp, SENSORIOCGPOLLRATE, 0); + return 1000000/TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); -- cgit v1.2.3 From 5fbee2394522d8b0c7a78d2751783845d011b56d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 11:17:29 +0200 Subject: Added flag to disable RC evaluation onboard of IO (raw values still forwarded) --- src/drivers/px4io/px4io.cpp | 31 +++++++++++++++++++++++-------- src/modules/px4iofirmware/mixer.cpp | 3 ++- src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 10 +++++++++- 4 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cebe33996..afbd4e695 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -206,7 +206,8 @@ private: unsigned _max_relays; /// Date: Wed, 21 Aug 2013 12:37:06 +0200 Subject: Changed range handling of LSM303D once again, added defines for default values --- src/drivers/lsm303d/lsm303d.cpp | 63 +++++++++++++++++++++++++---------------- 1 file changed, 39 insertions(+), 24 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2b7769992..803cd658f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,6 +54,7 @@ #include #include +#include #include #include @@ -168,6 +169,14 @@ static const int ERROR = -1; #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 +/* default values for this device */ +#define ACCEL_DEFAULT_RANGE_G 8 +#define ACCEL_DEFAULT_SAMPLERATE 800 +#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MAG_DEFAULT_RANGE_GA 2 +#define MAG_DEFAULT_SAMPLERATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -219,7 +228,7 @@ private: struct mag_report *_mag_reports; struct accel_scale _accel_scale; - unsigned _accel_range_g; + unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_filter_bandwith; @@ -418,22 +427,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), - _accel_range_g(8), + _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), - _accel_samplerate(800), - _accel_filter_bandwith(50), - _mag_range_ga(2), + _accel_samplerate(0), + _accel_filter_bandwith(0), + _mag_range_ga(0.0f), _mag_range_scale(0.0f), - _mag_samplerate(100), + _mag_samplerate(0), _accel_topic(-1), _mag_topic(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(800, 30), - _accel_filter_y(800, 30), - _accel_filter_z(800, 30) + _accel_filter_x(0, 0), + _accel_filter_y(0, 0), + _accel_filter_z(0, 0) { // enable debug() calls _debug_enabled = true; @@ -529,12 +538,17 @@ LSM303D::reset() write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - accel_set_range(_accel_range_g); - accel_set_samplerate(_accel_samplerate); - accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + _accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ; + + accel_set_range(ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE); + accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(MAG_DEFAULT_SAMPLERATE); - mag_set_range(_mag_range_ga); - mag_set_samplerate(_mag_samplerate); + _accel_read = 0; + _mag_read = 0; } int @@ -658,8 +672,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* Use 800Hz as it is filtered in the driver as well*/ - return ioctl(filp, SENSORIOCSPOLLRATE, 800); + return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE); /* adjust to a legal polling interval in Hz */ default: { @@ -759,10 +772,12 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } case ACCELIOCSRANGE: + /* arg needs to be in G */ return accel_set_range(arg); case ACCELIOCGRANGE: - return _accel_range_g; + /* convert to m/s^2 and return rounded in G */ + return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -999,27 +1014,27 @@ LSM303D::accel_set_range(unsigned max_g) max_g = 16; if (max_g <= 2) { - _accel_range_g = 2; + _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_g = 4; + _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_g = 6; + _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_g = 8; + _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_g = 16; + _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -1027,7 +1042,7 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * 9.80665f; + _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1272,7 +1287,7 @@ LSM303D::measure() accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_g * 9.80665f; + accel_report->range_m_s2 = _accel_range_m_s2; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); -- cgit v1.2.3 From 64b8f5232bcd12a819cb72e862158db6db5a0a66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 13:54:37 +0200 Subject: Build fix, added command line switch norc to disable RC --- src/drivers/px4io/px4io.cpp | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index afbd4e695..2e8946264 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -175,6 +175,11 @@ public: */ void print_status(); + /** + * Disable RC input handling + */ + int disable_rc_handling(); + /** * Set the DSM VCC is controlled by relay one flag * @@ -276,6 +281,11 @@ private: */ int io_get_status(); + /** + * Disable RC input handling + */ + int io_disable_rc_handling(); + /** * Fetch RC inputs from IO. * @@ -853,6 +863,12 @@ PX4IO::io_set_arming_state() return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); } +int +PX4IO::io_disable_rc_handling() +{ + return io_disable_rc_handling(); +} + int PX4IO::io_disable_rc_handling() { @@ -1785,6 +1801,16 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + /* disable RC handling on request */ + if (argc > 0 && !strcmp(argv[0], "norc")) { + + if(g_dev->disable_rc_handling()) + warnx("Failed disabling RC handling"); + + } else { + warnx("unknown argument: %s", argv[0]); + } + int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { -- cgit v1.2.3 From 7db420b9b222e6114e2cb6ffb5d726a946dd07c6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:20:42 +0200 Subject: Get units right in config --- src/systemcmds/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 262a52d20..188dafa4e 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -212,7 +212,7 @@ do_mag(int argc, char *argv[]) int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); - warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); close(fd); } @@ -279,7 +279,7 @@ do_accel(int argc, char *argv[]) int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range); + warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); close(fd); } -- cgit v1.2.3 From 8083efb60c97ffce5be8dcbf3956ab67cc17d729 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:11 +0200 Subject: Use gyro at correct rate --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index dda558b4c..2ffa5f698 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -919,11 +919,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif -- cgit v1.2.3 From 1ede95d252f5401f3bcf94265c42a060833ca8ca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:54 +0200 Subject: L3GD20 and LSM303D reset and range config working properly now --- src/drivers/l3gd20/l3gd20.cpp | 131 ++++++++++++++++++++++++++-------------- src/drivers/lsm303d/lsm303d.cpp | 92 ++++++++++++++++------------ 2 files changed, 136 insertions(+), 87 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index de6b753f1..5e0a2119a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -154,6 +154,10 @@ static const int ERROR = -1; #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) +#define L3GD20_DEFAULT_RATE 760 +#define L3GD20_DEFAULT_RANGE_DPS 2000 +#define L3GD20_DEFAULT_FILTER_FREQ 30 + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -191,9 +195,10 @@ private: orb_advert_t _gyro_topic; unsigned _current_rate; - unsigned _current_range; unsigned _orientation; + unsigned _read; + perf_counter_t _sample_perf; math::LowPassFilter2p _gyro_filter_x; @@ -210,6 +215,11 @@ private: */ void stop(); + /** + * Reset the driver + */ + void reset(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -273,6 +283,14 @@ private: */ int set_samplerate(unsigned frequency); + /** + * Set the lowpass filter of the driver + * + * @param samplerate The current samplerate + * @param frequency The cutoff frequency for the lowpass filter + */ + void set_driver_lowpass_filter(float samplerate, float bandwidth); + /** * Self test * @@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_rad_s(0.0f), _gyro_topic(-1), _current_rate(0), - _current_range(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), - _gyro_filter_x(250, 30), - _gyro_filter_y(250, 30), - _gyro_filter_z(250, 30) + _read(0), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -349,22 +367,7 @@ L3GD20::init() memset(&_reports[0], 0, sizeof(_reports[0])); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ - write_reg(ADDR_CTRL_REG4, REG4_BDU); - write_reg(ADDR_CTRL_REG5, 0); - - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - - /* disable FIFO. This makes things simpler and ensures we - * aren't getting stale data. It means we must run the hrt - * callback fast enough to not miss data. */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - - set_range(2000); /* default to 2000dps */ - set_samplerate(0); /* max sample rate */ + reset(); ret = OK; out: @@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -483,12 +485,10 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; - // adjust filters - float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* adjust filters */ + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ if (want_start) @@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _num_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; case GYROIOCSSAMPLERATE: return set_samplerate(arg); @@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _current_rate; case GYROIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_interval; - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + + return OK; + } case GYROIOCGLOWPASS: - return _gyro_filter_x.get_cutoff_freq(); + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: + /* arg should be in dps */ return set_range(arg); case GYROIOCGRANGE: - return _current_range; + /* convert to dps and round */ + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return self_test(); @@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; float new_range_scale_dps_digit; + float new_range; if (max_dps == 0) { max_dps = 2000; } if (max_dps <= 250) { - _current_range = 250; + new_range = 250; bits |= RANGE_250DPS; new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { - _current_range = 500; + new_range = 500; bits |= RANGE_500DPS; new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { - _current_range = 2000; + new_range = 2000; bits |= RANGE_2000DPS; new_range_scale_dps_digit = 70e-3f; @@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps) return -EINVAL; } - _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; + _gyro_range_rad_s = new_range / 180.0f * M_PI_F; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); @@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - // use limits good for H or non-H models + /* use limits good for H or non-H models */ if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; @@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency) return OK; } +void +L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth); +} + void L3GD20::start() { @@ -701,6 +711,30 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::reset() +{ + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG4, REG4_BDU); + write_reg(ADDR_CTRL_REG5, 0); + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); + + set_samplerate(L3GD20_DEFAULT_RATE); + set_range(L3GD20_DEFAULT_RANGE_DPS); + set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); + + _read = 0; +} + void L3GD20::measure_trampoline(void *arg) { @@ -804,6 +838,8 @@ L3GD20::measure() if (_gyro_topic > 0) orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + _read++; + /* stop the perf counter */ perf_end(_sample_perf); } @@ -811,6 +847,7 @@ L3GD20::measure() void L3GD20::print_info() { + printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); @@ -949,7 +986,7 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel pollrate reset failed"); exit(0); } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 803cd658f..efe7cf8eb 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -170,13 +170,13 @@ static const int ERROR = -1; #define INT_SRC_M 0x13 /* default values for this device */ -#define ACCEL_DEFAULT_RANGE_G 8 -#define ACCEL_DEFAULT_SAMPLERATE 800 -#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 -#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define LSM303D_ACCEL_DEFAULT_RANGE_G 8 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MAG_DEFAULT_RANGE_GA 2 -#define MAG_DEFAULT_SAMPLERATE 100 +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -231,7 +231,7 @@ private: unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; - unsigned _accel_filter_bandwith; + unsigned _accel_onchip_filter_bandwith; struct mag_scale _mag_scale; unsigned _mag_range_ga; @@ -356,13 +356,22 @@ private: int mag_set_range(unsigned max_g); /** - * Set the LSM303D accel anti-alias filter. + * Set the LSM303D on-chip anti-alias filter bandwith. * * @param bandwidth The anti-alias filter bandwidth in Hz * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int accel_set_antialias_filter_bandwidth(unsigned bandwith); + int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); + + /** + * Set the driver lowpass filter bandwidth. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -430,7 +439,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), - _accel_filter_bandwith(0), + _accel_onchip_filter_bandwith(0), _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), @@ -440,9 +449,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(0, 0), - _accel_filter_y(0, 0), - _accel_filter_z(0, 0) + _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -538,14 +547,13 @@ LSM303D::reset() write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - _accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - - accel_set_range(ACCEL_DEFAULT_RANGE_G); - accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE); - accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); + accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - mag_set_range(MAG_DEFAULT_RANGE_GA); - mag_set_samplerate(MAG_DEFAULT_SAMPLERATE); + mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); _accel_read = 0; _mag_read = 0; @@ -672,7 +680,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE); + return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -687,11 +695,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* adjust filters */ - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - - _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -750,10 +754,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - return OK; + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); } case ACCELIOCGLOWPASS: @@ -1091,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1101,19 +1102,19 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; - _accel_filter_bandwith = 50; + _accel_onchip_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; - _accel_filter_bandwith = 194; + _accel_onchip_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; - _accel_filter_bandwith = 362; + _accel_onchip_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; - _accel_filter_bandwith = 773; + _accel_onchip_filter_bandwith = 773; } else { return -EINVAL; @@ -1124,6 +1125,16 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) return OK; } +int +LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); + + return OK; +} + int LSM303D::accel_set_samplerate(unsigned frequency) { @@ -1582,15 +1593,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "accel driver poll rate reset failed"); + err(1, "accel pollrate reset failed"); - int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(MAG_DEVICE_PATH, O_RDONLY); - if (fd_mag < 0) { - warnx("could not open to mag " MAG_DEVICE_PATH); + if (fd < 0) { + warnx("mag could not be opened, external mag might be used"); } else { + /* no need to reset the mag as well, the reset() is the same */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "mag driver poll rate reset failed"); + err(1, "mag pollrate reset failed"); } exit(0); -- cgit v1.2.3 From 4f51f333a9a125ce137abe689c3fc0ce7943701b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:52:20 +0200 Subject: Adapted the MPU6000 to have the same get range ioctls and defines for defaults --- src/drivers/mpu6000/mpu6000.cpp | 87 +++++++++++++++++++++++------------------ 1 file changed, 48 insertions(+), 39 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bfc74c73e..0e65923db 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,15 @@ #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A +#define MPU6000_ACCEL_DEFAULT_RANGE_G 8 +#define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_GYRO_DEFAULT_RANGE_G 8 +#define MPU6000_GYRO_DEFAULT_RATE 1000 +#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 class MPU6000_gyro; @@ -357,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _reads(0), _sample_rate(1000), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), - _accel_filter_x(1000, 30), - _accel_filter_y(1000, 30), - _accel_filter_z(1000, 30), - _gyro_filter_x(1000, 30), - _gyro_filter_y(1000, 30), - _gyro_filter_z(1000, 30) + _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) { // disable debug() calls _debug_enabled = false; @@ -485,14 +494,13 @@ void MPU6000::reset() up_udelay(1000); // SAMPLE RATE - //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(_sample_rate); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response - _set_dlpf_filter(42); + _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -535,8 +543,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (9.81f / 4096.0f); - _accel_range_m_s2 = 8.0f * 9.81f; + _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; usleep(1000); @@ -777,9 +785,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + case SENSOR_POLLRATE_DEFAULT: - /* set to same as sample rate per default */ - return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate); + return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -867,9 +876,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // XXX decide on relationship of both filters // i.e. disable the on-chip filter //_set_dlpf_filter((uint16_t)arg); - _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: @@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return _accel_range_m_s2; + return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -920,28 +929,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; - return -ENOMEM; - } + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + GyroReportBuffer *buf = new GyroReportBuffer(arg); + + if (nullptr == buf) + return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; + } - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete _gyro_reports; + _gyro_reports = buf; + start(); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_rad_s = xx return -EINVAL; case GYROIOCGRANGE: - return _gyro_range_rad_s; + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); @@ -1400,7 +1409,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / CONSTANTS_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); -- cgit v1.2.3 From 5be2f4a792dab32a5fa25f4faaff1edf05143cd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 14:54:57 +0200 Subject: Moved mavlink log to system lib --- src/drivers/px4io/px4io.cpp | 4 +- src/modules/mavlink/mavlink_log.c | 129 ------------------------------------ src/modules/mavlink/module.mk | 1 - src/modules/systemlib/mavlink_log.c | 120 +++++++++++++++++++++++++++++++++ src/modules/systemlib/module.mk | 3 +- 5 files changed, 124 insertions(+), 133 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_log.c create mode 100644 src/modules/systemlib/mavlink_log.c diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2e8946264..c00816a12 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -864,7 +864,7 @@ PX4IO::io_set_arming_state() } int -PX4IO::io_disable_rc_handling() +PX4IO::disable_rc_handling() { return io_disable_rc_handling(); } @@ -1803,7 +1803,7 @@ start(int argc, char *argv[]) /* disable RC handling on request */ if (argc > 0 && !strcmp(argv[0], "norc")) { - + if(g_dev->disable_rc_handling()) warnx("Failed disabling RC handling"); diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c deleted file mode 100644 index 192195856..000000000 --- a/src/modules/mavlink/mavlink_log.c +++ /dev/null @@ -1,129 +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 mavlink_log.c - * MAVLink text logging. - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include - -#include - -static FILE* text_recorder_fd = NULL; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) -{ - lb->size = size; - lb->start = 0; - lb->count = 0; - lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); - text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); -} - -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) -{ - return lb->count == (int)lb->size; -} - -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) -{ - return lb->count == 0; -} - -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) -{ - int end = (lb->start + lb->count) % lb->size; - memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); - - if (mavlink_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - - } else { - ++lb->count; - } -} - -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) -{ - if (!mavlink_logbuffer_is_empty(lb)) { - memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); - lb->start = (lb->start + 1) % lb->size; - --lb->count; - - if (text_recorder_fd) { - fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); - fputc("\n", text_recorder_fd); - fsync(text_recorder_fd); - } - - return 0; - - } else { - return 1; - } -} - -void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) -{ - va_list ap; - va_start(ap, fmt); - int end = (lb->start + lb->count) % lb->size; - lb->elems[end].severity = severity; - vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap); - va_end(ap); - - /* increase count */ - if (mavlink_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - - } else { - ++lb->count; - } -} - -__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) -{ - va_list ap; - va_start(ap, fmt); - char text[MAVLINK_LOG_MAXLEN + 1]; - vsnprintf(text, sizeof(text), fmt, ap); - va_end(ap); - ioctl(_fd, severity, (unsigned long)&text[0]); -} diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index bfccb2d38..5d3d6a73c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ - mavlink_log.c \ mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c new file mode 100644 index 000000000..27608bdbf --- /dev/null +++ b/src/modules/systemlib/mavlink_log.c @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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_log.c + * MAVLink text logging. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include + +#include + +__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +{ + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +{ + return lb->count == (int)lb->size; +} + +__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +{ + return lb->count == 0; +} + +__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +{ + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +{ + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + + return 0; + + } else { + return 1; + } +} + +__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + int end = (lb->start + lb->count) % lb->size; + lb->elems[end].severity = severity; + vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap); + va_end(ap); + + /* increase count */ + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + char text[MAVLINK_LOG_MAXLEN + 1]; + vsnprintf(text, sizeof(text), fmt, ap); + va_end(ap); + ioctl(_fd, severity, (unsigned long)&text[0]); +} diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index b470c1227..cbf829122 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ geo/geo.c \ systemlib.c \ airspeed.c \ - system_params.c + system_params.c \ + mavlink_log.c -- cgit v1.2.3 From 2bcf4385f66b2bcdb2917d2edf60d40813e207df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 24 Jun 2013 12:39:33 +1000 Subject: build: use unqualified com port names on windows the qualified names were not working with current versions of python --- makefiles/upload.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index c55e3f188..470ddfdf1 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -18,7 +18,7 @@ ifeq ($(SYSTYPE),Linux) SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" endif .PHONY: all upload-$(METHOD)-$(BOARD) -- cgit v1.2.3 From f665ace38cfa4613fb911cb68f6662b15720ffea Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 11 Aug 2013 11:34:19 -1000 Subject: Add scripts for debugging with openocd Note: We now use the version of stm32f4x that comes with openocd 0.7.0 or later --- Debug/olimex-px4fmu-debug.cfg | 22 +++++++++++++++ Debug/openocd.gdbinit | 7 +++++ Debug/px4fmu-v1-board.cfg | 6 ++++ Debug/runopenocd.sh | 1 + Debug/stm32f4x.cfg | 64 ------------------------------------------- 5 files changed, 36 insertions(+), 64 deletions(-) create mode 100644 Debug/olimex-px4fmu-debug.cfg create mode 100644 Debug/openocd.gdbinit create mode 100644 Debug/px4fmu-v1-board.cfg create mode 100755 Debug/runopenocd.sh delete mode 100644 Debug/stm32f4x.cfg diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg new file mode 100644 index 000000000..61d70070d --- /dev/null +++ b/Debug/olimex-px4fmu-debug.cfg @@ -0,0 +1,22 @@ +# program a bootable device load on a mavstation +# To run type openocd -f mavprogram.cfg + +source [find interface/olimex-arm-usb-ocd-h.cfg] +source [find px4fmu-v1-board.cfg] + +init +halt + +# Find the flash inside this CPU +flash probe 0 + +# erase it (128 pages) then program and exit + +#flash erase_sector 0 0 127 +# stm32f1x mass_erase 0 + +# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around +#flash write_bank 0 fixed.bin 0 +#flash write_image firmware.elf +#shutdown + diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit new file mode 100644 index 000000000..4d2dc4c86 --- /dev/null +++ b/Debug/openocd.gdbinit @@ -0,0 +1,7 @@ +target remote :3333 +mon reset halt +mon poll +mon cortex_m maskisr auto +set mem inaccessible-by-default off +set print pretty +source Debug/PX4 \ No newline at end of file diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg new file mode 100644 index 000000000..ce1cca571 --- /dev/null +++ b/Debug/px4fmu-v1-board.cfg @@ -0,0 +1,6 @@ +# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu + +# increase working area to 32KB for faster flash programming +set WORKAREASIZE 0x8000 + +source [find target/stm32f4x.cfg] diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh new file mode 100755 index 000000000..291c5b0f5 --- /dev/null +++ b/Debug/runopenocd.sh @@ -0,0 +1 @@ +openocd -f interface/olimex-arm-usb-ocd-h.cfg -f Debug/stm32f4x.cfg diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg deleted file mode 100644 index 28bfcfbbb..000000000 --- a/Debug/stm32f4x.cfg +++ /dev/null @@ -1,64 +0,0 @@ -# script for stm32f2xxx - -if { [info exists CHIPNAME] } { - set _CHIPNAME $CHIPNAME -} else { - set _CHIPNAME stm32f4xxx -} - -if { [info exists ENDIAN] } { - set _ENDIAN $ENDIAN -} else { - set _ENDIAN little -} - -# Work-area is a space in RAM used for flash programming -# By default use 64kB -if { [info exists WORKAREASIZE] } { - set _WORKAREASIZE $WORKAREASIZE -} else { - set _WORKAREASIZE 0x10000 -} - -# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz -# -# Since we may be running of an RC oscilator, we crank down the speed a -# bit more to be on the safe side. Perhaps superstition, but if are -# running off a crystal, we can run closer to the limit. Note -# that there can be a pretty wide band where things are more or less stable. -jtag_khz 1000 - -jtag_nsrst_delay 100 -jtag_ntrst_delay 100 - -#jtag scan chain -if { [info exists CPUTAPID ] } { - set _CPUTAPID $CPUTAPID -} else { - # See STM Document RM0033 - # Section 32.6.3 - corresponds to Cortex-M3 r2p0 - set _CPUTAPID 0x4ba00477 -} -jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID - -if { [info exists BSTAPID ] } { - set _BSTAPID $BSTAPID -} else { - # See STM Document RM0033 - # Section 32.6.2 - # - set _BSTAPID 0x06413041 -} -jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID - -set _TARGETNAME $_CHIPNAME.cpu -target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto - -$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 - -set _FLASHNAME $_CHIPNAME.flash -flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME - -# if srst is not fitted use SYSRESETREQ to -# perform a soft reset -cortex_m3 reset_config sysresetreq -- cgit v1.2.3 From fa8f8f2a0255d743494e17120955421677e76567 Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 11 Aug 2013 11:38:00 -1000 Subject: add step hooks to make stepping work correctly for non isrs Conflicts: Debug/openocd.gdbinit Debug/px4fmu-v1-board.cfg --- Debug/openocd.gdbinit | 18 ++++++++++++++++-- Debug/px4fmu-v1-board.cfg | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit index 4d2dc4c86..92d78b58d 100644 --- a/Debug/openocd.gdbinit +++ b/Debug/openocd.gdbinit @@ -1,7 +1,21 @@ target remote :3333 -mon reset halt + +# Don't let GDB get confused while stepping +define hook-step + mon cortex_m maskisr on +end +define hookpost-step + mon cortex_m maskisr off +end + +mon init +mon stm32_init +# mon reset halt mon poll mon cortex_m maskisr auto set mem inaccessible-by-default off set print pretty -source Debug/PX4 \ No newline at end of file +source Debug/PX4 + +echo PX4 resumed, press ctrl-c to interrupt\n +continue diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg index ce1cca571..19b862a2d 100644 --- a/Debug/px4fmu-v1-board.cfg +++ b/Debug/px4fmu-v1-board.cfg @@ -4,3 +4,35 @@ set WORKAREASIZE 0x8000 source [find target/stm32f4x.cfg] + +# needed for px4 +reset_config trst_only + +proc stm32_reset {} { + reset halt +# FIXME - needed to init periphs on reset +# 0x40023800 RCC base +# 0x24 RCC_APB2 0x75933 +# RCC_APB2 0 +} + +# perform init that is required on each connection to the target +proc stm32_init {} { + + # force jtag to not shutdown during sleep + #uint32_t cr = getreg32(STM32_DBGMCU_CR); + #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP; + #putreg32(cr, STM32_DBGMCU_CR); + mww 0xe0042004 00000007 +} + +# if srst is not fitted use SYSRESETREQ to +# perform a soft reset +cortex_m reset_config sysresetreq + +# Let GDB directly program elf binaries +gdb_memory_map enable + +# doesn't work yet +gdb_flash_program disable + -- cgit v1.2.3 From 1c371a6cf81d3a791237d1969ce84f512670c6c9 Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sun, 11 Aug 2013 17:17:29 -1000 Subject: openocd: lost change from my cherry-picking --- Debug/runopenocd.sh | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh index 291c5b0f5..6258fccfb 100755 --- a/Debug/runopenocd.sh +++ b/Debug/runopenocd.sh @@ -1 +1,5 @@ -openocd -f interface/olimex-arm-usb-ocd-h.cfg -f Debug/stm32f4x.cfg +#!/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg -- cgit v1.2.3 From 5a8dc9c504f70b4ce1b45f91b3bdd9b7126ef0d3 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 19 Aug 2013 22:58:50 +0200 Subject: Added TBS script --- ROMFS/px4fmu_common/init.d/10_io_f330 | 9 ++- ROMFS/px4fmu_common/init.d/15_io_tbs | 138 ++++++++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 5 ++ 3 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/15_io_tbs diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 48636292c..0e6d3f5d5 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,17 @@ #!nsh # -# Flight startup script for PX4FMU+PX4IO +# Flight startup script for PX4FMU+PX4IO on an F330 quad. # # disable USB and autostart set USB no set MODE custom +# +# Start the ORB (first app to start) +# +uorb start + # # Load default params for this platform # @@ -49,7 +54,7 @@ usleep 5000 # Start the commander (depends on orb, mavlink) # commander start - + # # Start PX4IO interface (depends on orb, commander) # diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs new file mode 100644 index 000000000..237bb4267 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -0,0 +1,138 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad +# with stock DJI ESCs, motors and props. +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start +pwm -u 400 -m 0xff + +# +# Allow PX4IO to recover from midair restarts. +# This is very unlikely, but quite safe and robust. +px4io recovery + +# +# This sets a PWM right after startup (regardless of safety button) +# +px4io idle 900 900 900 900 + +# +# The values are for spinning motors when armed using DJI ESCs +# +px4io min 1180 1180 1180 1180 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Start the controllers (depends on orb) +# +multirotor_att_control start + +# +# Disable px4io topic limiting and start logging +# +if [ $BOARD == fmuv1 ] +then + px4io limit 200 + sdlog2 start -r 50 -a -b 16 + if blinkm start + then + blinkm systemstate + fi +else + px4io limit 400 + sdlog2 start -r 200 -a -b 16 + if rgbled start + then + #rgbled systemstate + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7f0409519..650224cf1 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -150,6 +150,11 @@ then sh /etc/init.d/10_io_f330 fi +if param compare SYS_AUTOSTART 15 +then + sh /etc/init.d/15_io_tbs +fi + if param compare SYS_AUTOSTART 30 then sh /etc/init.d/30_io_camflyer -- cgit v1.2.3 From fab110d21f147e5064ff140aadac649017fa466e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 18:13:01 +0200 Subject: Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs --- ROMFS/px4fmu_common/init.d/rc.hil | 12 +- makefiles/setup.mk | 4 +- src/drivers/hott/messages.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 17 +- src/drivers/mpu6000/mpu6000.cpp | 10 +- src/examples/fixedwing_control/main.c | 2 +- src/lib/geo/geo.c | 438 ++ src/lib/geo/geo.h | 129 + src/lib/geo/module.mk | 38 + .../CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h | 264 + .../CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h | 265 + src/lib/mathlib/CMSIS/Include/arm_common_tables.h | 93 + src/lib/mathlib/CMSIS/Include/arm_const_structs.h | 85 + src/lib/mathlib/CMSIS/Include/arm_math.h | 7318 ++++++++++++++++++++ src/lib/mathlib/CMSIS/Include/core_cm3.h | 1627 +++++ src/lib/mathlib/CMSIS/Include/core_cm4.h | 1772 +++++ src/lib/mathlib/CMSIS/Include/core_cm4_simd.h | 673 ++ src/lib/mathlib/CMSIS/Include/core_cmFunc.h | 636 ++ src/lib/mathlib/CMSIS/Include/core_cmInstr.h | 688 ++ src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a | Bin 0 -> 3039508 bytes src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a | Bin 0 -> 3049684 bytes src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a | Bin 0 -> 2989192 bytes src/lib/mathlib/CMSIS/library.mk | 46 + src/lib/mathlib/CMSIS/license.txt | 27 + src/lib/mathlib/math/Dcm.cpp | 174 + src/lib/mathlib/math/Dcm.hpp | 108 + src/lib/mathlib/math/EulerAngles.cpp | 126 + src/lib/mathlib/math/EulerAngles.hpp | 74 + src/lib/mathlib/math/Limits.cpp | 146 + src/lib/mathlib/math/Limits.hpp | 87 + src/lib/mathlib/math/Matrix.cpp | 193 + src/lib/mathlib/math/Matrix.hpp | 61 + src/lib/mathlib/math/Quaternion.cpp | 174 + src/lib/mathlib/math/Quaternion.hpp | 115 + src/lib/mathlib/math/Vector.cpp | 100 + src/lib/mathlib/math/Vector.hpp | 57 + src/lib/mathlib/math/Vector2f.cpp | 103 + src/lib/mathlib/math/Vector2f.hpp | 79 + src/lib/mathlib/math/Vector3.cpp | 99 + src/lib/mathlib/math/Vector3.hpp | 76 + src/lib/mathlib/math/arm/Matrix.cpp | 40 + src/lib/mathlib/math/arm/Matrix.hpp | 292 + src/lib/mathlib/math/arm/Vector.cpp | 40 + src/lib/mathlib/math/arm/Vector.hpp | 236 + src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 77 + src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 78 + src/lib/mathlib/math/filter/module.mk | 44 + src/lib/mathlib/math/generic/Matrix.cpp | 40 + src/lib/mathlib/math/generic/Matrix.hpp | 437 ++ src/lib/mathlib/math/generic/Vector.cpp | 40 + src/lib/mathlib/math/generic/Vector.hpp | 245 + src/lib/mathlib/math/nasa_rotation_def.pdf | Bin 0 -> 709235 bytes src/lib/mathlib/math/test/test.cpp | 94 + src/lib/mathlib/math/test/test.hpp | 50 + src/lib/mathlib/math/test_math.sce | 63 + src/lib/mathlib/mathlib.h | 59 + src/lib/mathlib/module.mk | 62 + .../commander/accelerometer_calibration.cpp | 2 +- src/modules/controllib/uorb/blocks.hpp | 2 +- .../CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h | 264 - .../CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h | 265 - .../mathlib/CMSIS/Include/arm_common_tables.h | 93 - .../mathlib/CMSIS/Include/arm_const_structs.h | 85 - src/modules/mathlib/CMSIS/Include/arm_math.h | 7318 -------------------- src/modules/mathlib/CMSIS/Include/core_cm3.h | 1627 ----- src/modules/mathlib/CMSIS/Include/core_cm4.h | 1772 ----- src/modules/mathlib/CMSIS/Include/core_cm4_simd.h | 673 -- src/modules/mathlib/CMSIS/Include/core_cmFunc.h | 636 -- src/modules/mathlib/CMSIS/Include/core_cmInstr.h | 688 -- src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a | Bin 3039508 -> 0 bytes src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a | Bin 3049684 -> 0 bytes src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a | Bin 2989192 -> 0 bytes src/modules/mathlib/CMSIS/library.mk | 46 - src/modules/mathlib/CMSIS/license.txt | 27 - src/modules/mathlib/math/Dcm.cpp | 174 - src/modules/mathlib/math/Dcm.hpp | 108 - src/modules/mathlib/math/EulerAngles.cpp | 126 - src/modules/mathlib/math/EulerAngles.hpp | 74 - src/modules/mathlib/math/Limits.cpp | 146 - src/modules/mathlib/math/Limits.hpp | 87 - src/modules/mathlib/math/Matrix.cpp | 193 - src/modules/mathlib/math/Matrix.hpp | 61 - src/modules/mathlib/math/Quaternion.cpp | 174 - src/modules/mathlib/math/Quaternion.hpp | 115 - src/modules/mathlib/math/Vector.cpp | 100 - src/modules/mathlib/math/Vector.hpp | 57 - src/modules/mathlib/math/Vector2f.cpp | 103 - src/modules/mathlib/math/Vector2f.hpp | 79 - src/modules/mathlib/math/Vector3.cpp | 99 - src/modules/mathlib/math/Vector3.hpp | 76 - src/modules/mathlib/math/arm/Matrix.cpp | 40 - src/modules/mathlib/math/arm/Matrix.hpp | 292 - src/modules/mathlib/math/arm/Vector.cpp | 40 - src/modules/mathlib/math/arm/Vector.hpp | 236 - .../mathlib/math/filter/LowPassFilter2p.cpp | 77 - .../mathlib/math/filter/LowPassFilter2p.hpp | 78 - src/modules/mathlib/math/filter/module.mk | 44 - src/modules/mathlib/math/generic/Matrix.cpp | 40 - src/modules/mathlib/math/generic/Matrix.hpp | 437 -- src/modules/mathlib/math/generic/Vector.cpp | 40 - src/modules/mathlib/math/generic/Vector.hpp | 245 - src/modules/mathlib/math/nasa_rotation_def.pdf | Bin 709235 -> 0 bytes src/modules/mathlib/math/test/test.cpp | 94 - src/modules/mathlib/math/test/test.hpp | 50 - src/modules/mathlib/math/test_math.sce | 63 - src/modules/mathlib/mathlib.h | 59 - src/modules/mathlib/module.mk | 62 - src/modules/mavlink/mavlink.c | 3 + src/modules/mavlink/mavlink_receiver.cpp | 88 +- src/modules/mavlink/orb_listener.c | 50 +- src/modules/mavlink/orb_topics.h | 1 + .../position_estimator_inav_main.c | 3 +- src/modules/systemlib/airspeed.c | 16 +- src/modules/systemlib/airspeed.h | 8 + src/modules/systemlib/conversions.c | 97 - src/modules/systemlib/conversions.h | 29 - src/modules/systemlib/geo/geo.c | 438 -- src/modules/systemlib/geo/geo.h | 129 - src/modules/systemlib/module.mk | 1 - 119 files changed, 17945 insertions(+), 17900 deletions(-) create mode 100644 src/lib/geo/geo.c create mode 100644 src/lib/geo/geo.h create mode 100644 src/lib/geo/module.mk create mode 100644 src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h create mode 100644 src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h create mode 100644 src/lib/mathlib/CMSIS/Include/arm_common_tables.h create mode 100644 src/lib/mathlib/CMSIS/Include/arm_const_structs.h create mode 100644 src/lib/mathlib/CMSIS/Include/arm_math.h create mode 100644 src/lib/mathlib/CMSIS/Include/core_cm3.h create mode 100644 src/lib/mathlib/CMSIS/Include/core_cm4.h create mode 100644 src/lib/mathlib/CMSIS/Include/core_cm4_simd.h create mode 100644 src/lib/mathlib/CMSIS/Include/core_cmFunc.h create mode 100644 src/lib/mathlib/CMSIS/Include/core_cmInstr.h create mode 100644 src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a create mode 100755 src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a create mode 100755 src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a create mode 100644 src/lib/mathlib/CMSIS/library.mk create mode 100644 src/lib/mathlib/CMSIS/license.txt create mode 100644 src/lib/mathlib/math/Dcm.cpp create mode 100644 src/lib/mathlib/math/Dcm.hpp create mode 100644 src/lib/mathlib/math/EulerAngles.cpp create mode 100644 src/lib/mathlib/math/EulerAngles.hpp create mode 100644 src/lib/mathlib/math/Limits.cpp create mode 100644 src/lib/mathlib/math/Limits.hpp create mode 100644 src/lib/mathlib/math/Matrix.cpp create mode 100644 src/lib/mathlib/math/Matrix.hpp create mode 100644 src/lib/mathlib/math/Quaternion.cpp create mode 100644 src/lib/mathlib/math/Quaternion.hpp create mode 100644 src/lib/mathlib/math/Vector.cpp create mode 100644 src/lib/mathlib/math/Vector.hpp create mode 100644 src/lib/mathlib/math/Vector2f.cpp create mode 100644 src/lib/mathlib/math/Vector2f.hpp create mode 100644 src/lib/mathlib/math/Vector3.cpp create mode 100644 src/lib/mathlib/math/Vector3.hpp create mode 100644 src/lib/mathlib/math/arm/Matrix.cpp create mode 100644 src/lib/mathlib/math/arm/Matrix.hpp create mode 100644 src/lib/mathlib/math/arm/Vector.cpp create mode 100644 src/lib/mathlib/math/arm/Vector.hpp create mode 100644 src/lib/mathlib/math/filter/LowPassFilter2p.cpp create mode 100644 src/lib/mathlib/math/filter/LowPassFilter2p.hpp create mode 100644 src/lib/mathlib/math/filter/module.mk create mode 100644 src/lib/mathlib/math/generic/Matrix.cpp create mode 100644 src/lib/mathlib/math/generic/Matrix.hpp create mode 100644 src/lib/mathlib/math/generic/Vector.cpp create mode 100644 src/lib/mathlib/math/generic/Vector.hpp create mode 100644 src/lib/mathlib/math/nasa_rotation_def.pdf create mode 100644 src/lib/mathlib/math/test/test.cpp create mode 100644 src/lib/mathlib/math/test/test.hpp create mode 100644 src/lib/mathlib/math/test_math.sce create mode 100644 src/lib/mathlib/mathlib.h create mode 100644 src/lib/mathlib/module.mk delete mode 100644 src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h delete mode 100644 src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h delete mode 100644 src/modules/mathlib/CMSIS/Include/arm_common_tables.h delete mode 100644 src/modules/mathlib/CMSIS/Include/arm_const_structs.h delete mode 100644 src/modules/mathlib/CMSIS/Include/arm_math.h delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cm3.h delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cm4.h delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cm4_simd.h delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cmFunc.h delete mode 100644 src/modules/mathlib/CMSIS/Include/core_cmInstr.h delete mode 100644 src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a delete mode 100755 src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a delete mode 100755 src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a delete mode 100644 src/modules/mathlib/CMSIS/library.mk delete mode 100644 src/modules/mathlib/CMSIS/license.txt delete mode 100644 src/modules/mathlib/math/Dcm.cpp delete mode 100644 src/modules/mathlib/math/Dcm.hpp delete mode 100644 src/modules/mathlib/math/EulerAngles.cpp delete mode 100644 src/modules/mathlib/math/EulerAngles.hpp delete mode 100644 src/modules/mathlib/math/Limits.cpp delete mode 100644 src/modules/mathlib/math/Limits.hpp delete mode 100644 src/modules/mathlib/math/Matrix.cpp delete mode 100644 src/modules/mathlib/math/Matrix.hpp delete mode 100644 src/modules/mathlib/math/Quaternion.cpp delete mode 100644 src/modules/mathlib/math/Quaternion.hpp delete mode 100644 src/modules/mathlib/math/Vector.cpp delete mode 100644 src/modules/mathlib/math/Vector.hpp delete mode 100644 src/modules/mathlib/math/Vector2f.cpp delete mode 100644 src/modules/mathlib/math/Vector2f.hpp delete mode 100644 src/modules/mathlib/math/Vector3.cpp delete mode 100644 src/modules/mathlib/math/Vector3.hpp delete mode 100644 src/modules/mathlib/math/arm/Matrix.cpp delete mode 100644 src/modules/mathlib/math/arm/Matrix.hpp delete mode 100644 src/modules/mathlib/math/arm/Vector.cpp delete mode 100644 src/modules/mathlib/math/arm/Vector.hpp delete mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.cpp delete mode 100644 src/modules/mathlib/math/filter/LowPassFilter2p.hpp delete mode 100644 src/modules/mathlib/math/filter/module.mk delete mode 100644 src/modules/mathlib/math/generic/Matrix.cpp delete mode 100644 src/modules/mathlib/math/generic/Matrix.hpp delete mode 100644 src/modules/mathlib/math/generic/Vector.cpp delete mode 100644 src/modules/mathlib/math/generic/Vector.hpp delete mode 100644 src/modules/mathlib/math/nasa_rotation_def.pdf delete mode 100644 src/modules/mathlib/math/test/test.cpp delete mode 100644 src/modules/mathlib/math/test/test.hpp delete mode 100644 src/modules/mathlib/math/test_math.sce delete mode 100644 src/modules/mathlib/mathlib.h delete mode 100644 src/modules/mathlib/module.mk delete mode 100644 src/modules/systemlib/geo/geo.c delete mode 100644 src/modules/systemlib/geo/geo.h diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index 3517a5bd8..a843b7ffb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -8,7 +8,7 @@ echo "[HIL] starting.." uorb start # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console +mavlink start -b 230400 -d /dev/ttyS1 # Create a fake HIL /dev/pwm_output interface hil mode_pwm @@ -38,13 +38,13 @@ commander start # # Check if we got an IO # -if [ px4io start ] +if px4io start then echo "IO started" else fmu mode_serial echo "FMU started" -end +fi # # Start the sensors (depends on orb, px4io) @@ -60,9 +60,7 @@ att_pos_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fixedwing_backside start +fw_pos_control_l1 start +fw_att_control start echo "[HIL] setup done, running" - -# Exit shell to make it available to MAVLink -exit diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5c..42f9a8a7f 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -43,6 +43,7 @@ # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ +export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ @@ -57,7 +58,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ # export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ - $(PX4_INCLUDE_DIR) + $(PX4_INCLUDE_DIR) \ + $(PX4_LIB_DIR) # # Tools diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 57c256339..bb8d45bea 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index efe7cf8eb..cf5f8d94c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,7 +54,6 @@ #include #include -#include #include #include @@ -178,6 +177,8 @@ static const int ERROR = -1; #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 +#define LSM303D_ONE_G 9.80665f + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -778,7 +779,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -1015,27 +1016,27 @@ LSM303D::accel_set_range(unsigned max_g) max_g = 16; if (max_g <= 2) { - _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 2.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 4.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 6.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 8.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 16.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -1043,7 +1044,7 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; + _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0e65923db..14f8f44b8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -159,6 +159,8 @@ #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 +#define MPU6000_ONE_G 9.80665f + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -543,8 +545,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; usleep(1000); @@ -906,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1409,7 +1411,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / CONSTANTS_ONE_G)); + (double)(a_report.range_m_s2 / MPU6000_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index e29f76877..b286e0007 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -66,7 +66,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c new file mode 100644 index 000000000..63792dda5 --- /dev/null +++ b/src/lib/geo/geo.c @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include + + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0d * M_PI; + double lon_now_rad = lon_now / 180.0d * M_PI; + double lat_next_rad = lat_next / 180.0d * M_PI; + double lon_next_rad = lon_next / 180.0d * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); + + const double radius_earth = 6371000.0d; + return radius_earth * c; +} + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + theta = _wrap_pi(theta); + + return theta; +} + +// Additional functions - @author Doug Weibel + +__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrap_pi(bearing_diff); + + // Return past_end = true if past end point of line + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + crosstrack_error->past_end = true; + return_value = OK; + return return_value; + } + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + + if (sin(bearing_diff) >= 0) { + crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); + + } else { + crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); + } + + return_value = OK; + + return return_value; + +} + + +__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; + + + if (arc_sweep >= 0) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + + if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + + if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; + } + + in_sector = false; + + // Case where sector does not span zero + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + + // Case where sector does span zero + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + + // If in the sector then calculate distance and bearing to closest point + if (in_sector) { + crosstrack_error->past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if (dist_to_center <= radius) { + crosstrack_error->distance = radius - dist_to_center; + crosstrack_error->bearing = bearing_now + M_PI_F; + + } else { + crosstrack_error->distance = dist_to_center - radius; + crosstrack_error->bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile + float start_disp_x = radius * sin(arc_start_bearing); + float start_disp_y = radius * cos(arc_start_bearing); + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0d; + float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; + float lon_end = lon_now + end_disp_x / 111111.0d; + float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + + + if (dist_to_start < dist_to_end) { + crosstrack_error->distance = dist_to_start; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + + } else { + crosstrack_error->past_end = true; + crosstrack_error->distance = dist_to_end; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } + + } + + crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + return_value = OK; + return return_value; +} + +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing) || bearing == 0) { + return bearing; + } + + int c = 0; + + while (bearing > M_PI_F && c < 30) { + bearing -= M_TWOPI_F; + c++; + } + + c = 0; + + while (bearing <= -M_PI_F && c < 30) { + bearing += M_TWOPI_F; + c++; + } + + return bearing; +} + +__EXPORT float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +__EXPORT float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + +__EXPORT float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + + diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h new file mode 100644 index 000000000..dadec51ec --- /dev/null +++ b/src/lib/geo/geo.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * Additional functions - @author Doug Weibel + */ + +#pragma once + +__BEGIN_DECLS + +#include + +#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ +#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ +#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ + +/* compatibility aliases */ +#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH +#define GRAVITY_MSS CONSTANTS_ONE_G + +// XXX remove +struct crosstrack_error_s { + bool past_end; // Flag indicating we are past the end of the line/arc segment + float distance; // Distance in meters to closest point on line/arc + float bearing; // Bearing in radians to closest point on line/arc +} ; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_init(double lat_0, double lon_0); + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); + +/** + * Returns the distance to the next waypoint in meters. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +/** + * Returns the bearing to the next waypoint in radians. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); + +__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + +__EXPORT float _wrap_180(float bearing); +__EXPORT float _wrap_360(float bearing); +__EXPORT float _wrap_pi(float bearing); +__EXPORT float _wrap_2pi(float bearing); + +__END_DECLS diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk new file mode 100644 index 000000000..30a2dc99f --- /dev/null +++ b/src/lib/geo/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Geo library +# + +SRCS = geo.c diff --git a/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h new file mode 100644 index 000000000..8f39acd9d --- /dev/null +++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h @@ -0,0 +1,264 @@ +/**************************************************************************//** + * @file ARMCM3.h + * @brief CMSIS Core Peripheral Access Layer Header File for + * ARMCM3 Device Series + * @version V1.07 + * @date 30. January 2012 + * + * @note + * Copyright (C) 2012 ARM Limited. All rights reserved. + * + * @par + * ARM Limited (ARM) is supplying this software for use with Cortex-M + * processor based microcontrollers. This file can be freely distributed + * within development tools that are supporting such ARM based processors. + * + * @par + * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED + * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. + * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. + * + ******************************************************************************/ + +#ifndef ARMCM3_H +#define ARMCM3_H + +#ifdef __cplusplus +extern "C" { +#endif + + +/* ------------------------- Interrupt Number Definition ------------------------ */ + +typedef enum IRQn +{ +/* ------------------- Cortex-M3 Processor Exceptions Numbers ------------------- */ + NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */ + HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */ + MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */ + BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */ + UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */ + SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */ + DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */ + PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */ + SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */ + +/* ---------------------- ARMCM3 Specific Interrupt Numbers --------------------- */ + WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */ + RTC_IRQn = 1, /*!< Real Time Clock Interrupt */ + TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */ + TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */ + MCIA_IRQn = 4, /*!< MCIa Interrupt */ + MCIB_IRQn = 5, /*!< MCIb Interrupt */ + UART0_IRQn = 6, /*!< UART0 Interrupt */ + UART1_IRQn = 7, /*!< UART1 Interrupt */ + UART2_IRQn = 8, /*!< UART2 Interrupt */ + UART4_IRQn = 9, /*!< UART4 Interrupt */ + AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */ + CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */ + ENET_IRQn = 12, /*!< Ethernet Interrupt */ + USBDC_IRQn = 13, /*!< USB Device Interrupt */ + USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */ + CHLCD_IRQn = 15, /*!< Character LCD Interrupt */ + FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */ + CAN_IRQn = 17, /*!< CAN Interrupt */ + LIN_IRQn = 18, /*!< LIN Interrupt */ + I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */ + CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */ + UART3_IRQn = 30, /*!< UART3 Interrupt */ + SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */ +} IRQn_Type; + + +/* ================================================================================ */ +/* ================ Processor and Core Peripheral Section ================ */ +/* ================================================================================ */ + +/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */ +#define __CM3_REV 0x0201 /*!< Core revision r2p1 */ +#define __MPU_PRESENT 1 /*!< MPU present or not */ +#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */ +#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */ + +#include /* Processor and core peripherals */ +/* NuttX */ +//#include "system_ARMCM3.h" /* System Header */ + + +/* ================================================================================ */ +/* ================ Device Specific Peripheral Section ================ */ +/* ================================================================================ */ + +/* ------------------- Start of section using anonymous unions ------------------ */ +#if defined(__CC_ARM) + #pragma push + #pragma anon_unions +#elif defined(__ICCARM__) + #pragma language=extended +#elif defined(__GNUC__) + /* anonymous unions are enabled by default */ +#elif defined(__TMS470__) +/* anonymous unions are enabled by default */ +#elif defined(__TASKING__) + #pragma warning 586 +#else + #warning Not supported compiler type +#endif + + + +/* ================================================================================ */ +/* ================ CPU FPGA System (CPU_SYS) ================ */ +/* ================================================================================ */ +typedef struct +{ + __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ + __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */ + __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ + __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ + __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */ + __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */ + uint32_t RESERVED0[2]; + __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */ + __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */ + __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */ + uint32_t RESERVED1[3]; + __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */ + __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */ +} ARM_CPU_SYS_TypeDef; + + +/* ================================================================================ */ +/* ================ DUT FPGA System (DUT_SYS) ================ */ +/* ================================================================================ */ +typedef struct +{ + __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ + __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */ + __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ + __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ + __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */ + __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */ + __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */ +} ARM_DUT_SYS_TypeDef; + + +/* ================================================================================ */ +/* ================ Timer (TIM) ================ */ +/* ================================================================================ */ +typedef struct +{ + __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */ + __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */ + __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */ + __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */ + __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */ + __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */ + __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */ + uint32_t RESERVED0[1]; + __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */ + __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */ + __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */ + __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */ + __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */ + __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */ + __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */ +} ARM_TIM_TypeDef; + + +/* ================================================================================ */ +/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */ +/* ================================================================================ */ +typedef struct +{ + __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */ + union { + __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */ + __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */ + }; + uint32_t RESERVED0[4]; + __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */ + uint32_t RESERVED1[1]; + __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */ + __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */ + __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */ + __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */ + __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */ + __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */ + __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */ + __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */ + __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */ + __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */ + __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */ +} ARM_UART_TypeDef; + + +/* -------------------- End of section using anonymous unions ------------------- */ +#if defined(__CC_ARM) + #pragma pop +#elif defined(__ICCARM__) + /* leave anonymous unions enabled */ +#elif defined(__GNUC__) + /* anonymous unions are enabled by default */ +#elif defined(__TMS470__) + /* anonymous unions are enabled by default */ +#elif defined(__TASKING__) + #pragma warning restore +#else + #warning Not supported compiler type +#endif + + + + +/* ================================================================================ */ +/* ================ Peripheral memory map ================ */ +/* ================================================================================ */ +/* -------------------------- CPU FPGA memory map ------------------------------- */ +#define ARM_FLASH_BASE (0x00000000UL) +#define ARM_RAM_BASE (0x20000000UL) +#define ARM_RAM_FPGA_BASE (0x1EFF0000UL) +#define ARM_CPU_CFG_BASE (0xDFFF0000UL) + +#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000) +#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000) + +/* -------------------------- DUT FPGA memory map ------------------------------- */ +#define ARM_APB_BASE (0x40000000UL) +#define ARM_AHB_BASE (0x4FF00000UL) +#define ARM_DMC_BASE (0x60000000UL) +#define ARM_SMC_BASE (0xA0000000UL) + +#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000) +#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000) +#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000) +#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000) +#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000) +#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000) +#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000) + + +/* ================================================================================ */ +/* ================ Peripheral declaration ================ */ +/* ================================================================================ */ +/* -------------------------- CPU FPGA Peripherals ------------------------------ */ +#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE) +#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE) + +/* -------------------------- DUT FPGA Peripherals ------------------------------ */ +#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE) +#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE) +#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE) +#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE) +#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE) +#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE) +#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE) + + +#ifdef __cplusplus +} +#endif + +#endif /* ARMCM3_H */ diff --git a/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h new file mode 100644 index 000000000..181b7e433 --- /dev/null +++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h @@ -0,0 +1,265 @@ +/**************************************************************************//** + * @file ARMCM4.h + * @brief CMSIS Core Peripheral Access Layer Header File for + * ARMCM4 Device Series + * @version V1.07 + * @date 30. January 2012 + * + * @note + * Copyright (C) 2012 ARM Limited. All rights reserved. + * + * @par + * ARM Limited (ARM) is supplying this software for use with Cortex-M + * processor based microcontrollers. This file can be freely distributed + * within development tools that are supporting such ARM based processors. + * + * @par + * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED + * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. + * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. + * + ******************************************************************************/ + +#ifndef ARMCM4_H +#define ARMCM4_H + +#ifdef __cplusplus +extern "C" { +#endif + + +/* ------------------------- Interrupt Number Definition ------------------------ */ + +typedef enum IRQn +{ +/* ------------------- Cortex-M4 Processor Exceptions Numbers ------------------- */ + NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */ + HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */ + MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */ + BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */ + UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */ + SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */ + DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */ + PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */ + SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */ + +/* ---------------------- ARMCM4 Specific Interrupt Numbers --------------------- */ + WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */ + RTC_IRQn = 1, /*!< Real Time Clock Interrupt */ + TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */ + TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */ + MCIA_IRQn = 4, /*!< MCIa Interrupt */ + MCIB_IRQn = 5, /*!< MCIb Interrupt */ + UART0_IRQn = 6, /*!< UART0 Interrupt */ + UART1_IRQn = 7, /*!< UART1 Interrupt */ + UART2_IRQn = 8, /*!< UART2 Interrupt */ + UART4_IRQn = 9, /*!< UART4 Interrupt */ + AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */ + CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */ + ENET_IRQn = 12, /*!< Ethernet Interrupt */ + USBDC_IRQn = 13, /*!< USB Device Interrupt */ + USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */ + CHLCD_IRQn = 15, /*!< Character LCD Interrupt */ + FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */ + CAN_IRQn = 17, /*!< CAN Interrupt */ + LIN_IRQn = 18, /*!< LIN Interrupt */ + I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */ + CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */ + UART3_IRQn = 30, /*!< UART3 Interrupt */ + SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */ +} IRQn_Type; + + +/* ================================================================================ */ +/* ================ Processor and Core Peripheral Section ================ */ +/* ================================================================================ */ + +/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */ +#define __CM4_REV 0x0001 /*!< Core revision r0p1 */ +#define __MPU_PRESENT 1 /*!< MPU present or not */ +#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */ +#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */ +#define __FPU_PRESENT 1 /*!< FPU present or not */ + +#include /* Processor and core peripherals */ +/* NuttX */ +//#include "system_ARMCM4.h" /* System Header */ + + +/* ================================================================================ */ +/* ================ Device Specific Peripheral Section ================ */ +/* ================================================================================ */ + +/* ------------------- Start of section using anonymous unions ------------------ */ +#if defined(__CC_ARM) + #pragma push + #pragma anon_unions +#elif defined(__ICCARM__) + #pragma language=extended +#elif defined(__GNUC__) + /* anonymous unions are enabled by default */ +#elif defined(__TMS470__) +/* anonymous unions are enabled by default */ +#elif defined(__TASKING__) + #pragma warning 586 +#else + #warning Not supported compiler type +#endif + + + +/* ================================================================================ */ +/* ================ CPU FPGA System (CPU_SYS) ================ */ +/* ================================================================================ */ +typedef struct +{ + __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ + __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */ + __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ + __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ + __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */ + __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */ + uint32_t RESERVED0[2]; + __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */ + __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */ + __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */ + uint32_t RESERVED1[3]; + __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */ + __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */ +} ARM_CPU_SYS_TypeDef; + + +/* ================================================================================ */ +/* ================ DUT FPGA System (DUT_SYS) ================ */ +/* ================================================================================ */ +typedef struct +{ + __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ + __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */ + __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ + __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ + __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */ + __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */ + __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */ +} ARM_DUT_SYS_TypeDef; + + +/* ================================================================================ */ +/* ================ Timer (TIM) ================ */ +/* ================================================================================ */ +typedef struct +{ + __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */ + __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */ + __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */ + __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */ + __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */ + __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */ + __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */ + uint32_t RESERVED0[1]; + __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */ + __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */ + __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */ + __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */ + __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */ + __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */ + __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */ +} ARM_TIM_TypeDef; + + +/* ================================================================================ */ +/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */ +/* ================================================================================ */ +typedef struct +{ + __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */ + union { + __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */ + __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */ + }; + uint32_t RESERVED0[4]; + __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */ + uint32_t RESERVED1[1]; + __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */ + __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */ + __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */ + __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */ + __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */ + __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */ + __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */ + __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */ + __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */ + __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */ + __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */ +} ARM_UART_TypeDef; + + +/* -------------------- End of section using anonymous unions ------------------- */ +#if defined(__CC_ARM) + #pragma pop +#elif defined(__ICCARM__) + /* leave anonymous unions enabled */ +#elif defined(__GNUC__) + /* anonymous unions are enabled by default */ +#elif defined(__TMS470__) + /* anonymous unions are enabled by default */ +#elif defined(__TASKING__) + #pragma warning restore +#else + #warning Not supported compiler type +#endif + + + + +/* ================================================================================ */ +/* ================ Peripheral memory map ================ */ +/* ================================================================================ */ +/* -------------------------- CPU FPGA memory map ------------------------------- */ +#define ARM_FLASH_BASE (0x00000000UL) +#define ARM_RAM_BASE (0x20000000UL) +#define ARM_RAM_FPGA_BASE (0x1EFF0000UL) +#define ARM_CPU_CFG_BASE (0xDFFF0000UL) + +#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000) +#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000) + +/* -------------------------- DUT FPGA memory map ------------------------------- */ +#define ARM_APB_BASE (0x40000000UL) +#define ARM_AHB_BASE (0x4FF00000UL) +#define ARM_DMC_BASE (0x60000000UL) +#define ARM_SMC_BASE (0xA0000000UL) + +#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000) +#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000) +#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000) +#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000) +#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000) +#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000) +#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000) + + +/* ================================================================================ */ +/* ================ Peripheral declaration ================ */ +/* ================================================================================ */ +/* -------------------------- CPU FPGA Peripherals ------------------------------ */ +#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE) +#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE) + +/* -------------------------- DUT FPGA Peripherals ------------------------------ */ +#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE) +#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE) +#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE) +#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE) +#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE) +#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE) +#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE) + + +#ifdef __cplusplus +} +#endif + +#endif /* ARMCM4_H */ diff --git a/src/lib/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h new file mode 100644 index 000000000..9c37ab4e5 --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_common_tables.h +* +* Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions +* +* Target Processor: Cortex-M4/Cortex-M3 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - 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. +* - Neither the name of ARM LIMITED 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 _ARM_COMMON_TABLES_H +#define _ARM_COMMON_TABLES_H + +#include "arm_math.h" + +extern const uint16_t armBitRevTable[1024]; +extern const q15_t armRecipTableQ15[64]; +extern const q31_t armRecipTableQ31[64]; +extern const q31_t realCoefAQ31[1024]; +extern const q31_t realCoefBQ31[1024]; +extern const float32_t twiddleCoef_16[32]; +extern const float32_t twiddleCoef_32[64]; +extern const float32_t twiddleCoef_64[128]; +extern const float32_t twiddleCoef_128[256]; +extern const float32_t twiddleCoef_256[512]; +extern const float32_t twiddleCoef_512[1024]; +extern const float32_t twiddleCoef_1024[2048]; +extern const float32_t twiddleCoef_2048[4096]; +extern const float32_t twiddleCoef_4096[8192]; +#define twiddleCoef twiddleCoef_4096 +extern const q31_t twiddleCoefQ31[6144]; +extern const q15_t twiddleCoefQ15[6144]; +extern const float32_t twiddleCoef_rfft_32[32]; +extern const float32_t twiddleCoef_rfft_64[64]; +extern const float32_t twiddleCoef_rfft_128[128]; +extern const float32_t twiddleCoef_rfft_256[256]; +extern const float32_t twiddleCoef_rfft_512[512]; +extern const float32_t twiddleCoef_rfft_1024[1024]; +extern const float32_t twiddleCoef_rfft_2048[2048]; +extern const float32_t twiddleCoef_rfft_4096[4096]; + + +#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 ) +#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 ) +#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 ) +#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 ) +#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 ) +#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 ) +#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800) +#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808) +#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032) + +extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH]; +extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH]; + +#endif /* ARM_COMMON_TABLES_H */ diff --git a/src/lib/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h new file mode 100644 index 000000000..406f737dc --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_const_structs.h +* +* Description: This file has constant structs that are initialized for +* user convenience. For example, some can be given as +* arguments to the arm_cfft_f32() function. +* +* Target Processor: Cortex-M4/Cortex-M3 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - 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. +* - Neither the name of ARM LIMITED 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 _ARM_CONST_STRUCTS_H +#define _ARM_CONST_STRUCTS_H + +#include "arm_math.h" +#include "arm_common_tables.h" + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = { + 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = { + 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = { + 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = { + 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = { + 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = { + 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = { + 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = { + 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH + }; + + const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = { + 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH + }; + +#endif diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h new file mode 100644 index 000000000..6f66f9ee3 --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/arm_math.h @@ -0,0 +1,7318 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_math.h +* +* Description: Public header file for CMSIS DSP Library +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - 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. +* - Neither the name of ARM LIMITED 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. + * -------------------------------------------------------------------- */ + +/** + \mainpage CMSIS DSP Software Library + * + * Introduction + * + * This user manual describes the CMSIS DSP software library, + * a suite of common signal processing functions for use on Cortex-M processor based devices. + * + * The library is divided into a number of functions each covering a specific category: + * - Basic math functions + * - Fast math functions + * - Complex math functions + * - Filters + * - Matrix functions + * - Transforms + * - Motor control functions + * - Statistical functions + * - Support functions + * - Interpolation functions + * + * The library has separate functions for operating on 8-bit integers, 16-bit integers, + * 32-bit integer and 32-bit floating-point values. + * + * Using the Library + * + * The library installer contains prebuilt versions of the libraries in the Lib folder. + * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4) + * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4) + * - arm_cortexM4l_math.lib (Little endian on Cortex-M4) + * - arm_cortexM4b_math.lib (Big endian on Cortex-M4) + * - arm_cortexM3l_math.lib (Little endian on Cortex-M3) + * - arm_cortexM3b_math.lib (Big endian on Cortex-M3) + * - arm_cortexM0l_math.lib (Little endian on Cortex-M0) + * - arm_cortexM0b_math.lib (Big endian on Cortex-M3) + * + * The library functions are declared in the public file arm_math.h which is placed in the Include folder. + * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single + * public header file arm_math.h for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants. + * Define the appropriate pre processor MACRO ARM_MATH_CM4 or ARM_MATH_CM3 or + * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application. + * + * Examples + * + * The library ships with a number of examples which demonstrate how to use the library functions. + * + * Toolchain Support + * + * The library has been developed and tested with MDK-ARM version 4.60. + * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly. + * + * Building the Library + * + * The library installer contains project files to re build libraries on MDK Tool chain in the CMSIS\\DSP_Lib\\Source\\ARM folder. + * - arm_cortexM0b_math.uvproj + * - arm_cortexM0l_math.uvproj + * - arm_cortexM3b_math.uvproj + * - arm_cortexM3l_math.uvproj + * - arm_cortexM4b_math.uvproj + * - arm_cortexM4l_math.uvproj + * - arm_cortexM4bf_math.uvproj + * - arm_cortexM4lf_math.uvproj + * + * + * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above. + * + * Pre-processor Macros + * + * Each library project have differant pre-processor macros. + * + * - UNALIGNED_SUPPORT_DISABLE: + * + * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access + * + * - ARM_MATH_BIG_ENDIAN: + * + * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets. + * + * - ARM_MATH_MATRIX_CHECK: + * + * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices + * + * - ARM_MATH_ROUNDING: + * + * Define macro ARM_MATH_ROUNDING for rounding on support functions + * + * - ARM_MATH_CMx: + * + * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target + * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target. + * + * - __FPU_PRESENT: + * + * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries + * + * Copyright Notice + * + * Copyright (C) 2010-2013 ARM Limited. All rights reserved. + */ + + +/** + * @defgroup groupMath Basic Math Functions + */ + +/** + * @defgroup groupFastMath Fast Math Functions + * This set of functions provides a fast approximation to sine, cosine, and square root. + * As compared to most of the other functions in the CMSIS math library, the fast math functions + * operate on individual values and not arrays. + * There are separate functions for Q15, Q31, and floating-point data. + * + */ + +/** + * @defgroup groupCmplxMath Complex Math Functions + * This set of functions operates on complex data vectors. + * The data in the complex arrays is stored in an interleaved fashion + * (real, imag, real, imag, ...). + * In the API functions, the number of samples in a complex array refers + * to the number of complex values; the array contains twice this number of + * real values. + */ + +/** + * @defgroup groupFilters Filtering Functions + */ + +/** + * @defgroup groupMatrix Matrix Functions + * + * This set of functions provides basic matrix math operations. + * The functions operate on matrix data structures. For example, + * the type + * definition for the floating-point matrix structure is shown + * below: + * 
+ *     typedef struct
+ *     {
+ *       uint16_t numRows;     // number of rows of the matrix.
+ *       uint16_t numCols;     // number of columns of the matrix.
+ *       float32_t *pData;     // points to the data of the matrix.
+ *     } arm_matrix_instance_f32;
+ *
+ * There are similar definitions for Q15 and Q31 data types. + * + * The structure specifies the size of the matrix and then points to + * an array of data. The array is of size numRows X numCols + * and the values are arranged in row order. That is, the + * matrix element (i, j) is stored at: + * 
+ *     pData[i*numCols + j]
+ *
+ * + * \par Init Functions + * There is an associated initialization function for each type of matrix + * data structure. + * The initialization function sets the values of the internal structure fields. + * Refer to the function arm_mat_init_f32(), arm_mat_init_q31() + * and arm_mat_init_q15() for floating-point, Q31 and Q15 types, respectively. + * + * \par + * Use of the initialization function is optional. However, if initialization function is used + * then the instance structure cannot be placed into a const data section. + * To place the instance structure in a const data + * section, manually initialize the data structure. For example: + * 
+ * arm_matrix_instance_f32 S = {nRows, nColumns, pData};
+ * arm_matrix_instance_q31 S = {nRows, nColumns, pData};
+ * arm_matrix_instance_q15 S = {nRows, nColumns, pData};
+ *
+ * where nRows specifies the number of rows, nColumns + * specifies the number of columns, and pData points to the + * data array. + * + * \par Size Checking + * By default all of the matrix functions perform size checking on the input and + * output matrices. For example, the matrix addition function verifies that the + * two input matrices and the output matrix all have the same number of rows and + * columns. If the size check fails the functions return: + * 
+ *     ARM_MATH_SIZE_MISMATCH
+ *
+ * Otherwise the functions return + * 
+ *     ARM_MATH_SUCCESS
+ *
+ * There is some overhead associated with this matrix size checking. + * The matrix size checking is enabled via the \#define + * 
+ *     ARM_MATH_MATRIX_CHECK
+ *
+ * within the library project settings. By default this macro is defined + * and size checking is enabled. By changing the project settings and + * undefining this macro size checking is eliminated and the functions + * run a bit faster. With size checking disabled the functions always + * return ARM_MATH_SUCCESS. + */ + +/** + * @defgroup groupTransforms Transform Functions + */ + +/** + * @defgroup groupController Controller Functions + */ + +/** + * @defgroup groupStats Statistics Functions + */ +/** + * @defgroup groupSupport Support Functions + */ + +/** + * @defgroup groupInterpolation Interpolation Functions + * These functions perform 1- and 2-dimensional interpolation of data. + * Linear interpolation is used for 1-dimensional data and + * bilinear interpolation is used for 2-dimensional data. + */ + +/** + * @defgroup groupExamples Examples + */ +#ifndef _ARM_MATH_H +#define _ARM_MATH_H + +#define __CMSIS_GENERIC /* disable NVIC and Systick functions */ + +/* PX4 */ +#include +#ifdef CONFIG_ARCH_CORTEXM4 +# define ARM_MATH_CM4 1 +#endif +#ifdef CONFIG_ARCH_CORTEXM3 +# define ARM_MATH_CM3 1 +#endif +#ifdef CONFIG_ARCH_FPU +# define __FPU_PRESENT 1 +#endif + +#if defined (ARM_MATH_CM4) +#include "core_cm4.h" +#elif defined (ARM_MATH_CM3) +#include "core_cm3.h" +#elif defined (ARM_MATH_CM0) +#include "core_cm0.h" +#define ARM_MATH_CM0_FAMILY +#elif defined (ARM_MATH_CM0PLUS) +#include "core_cm0plus.h" +#define ARM_MATH_CM0_FAMILY +#else +#include "ARMCM4.h" +#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....." +#endif + +#undef __CMSIS_GENERIC /* enable NVIC and Systick functions */ +#include "string.h" +#include "math.h" +#ifdef __cplusplus +extern "C" +{ +#endif + + + /** + * @brief Macros required for reciprocal calculation in Normalized LMS + */ + +#define DELTA_Q31 (0x100) +#define DELTA_Q15 0x5 +#define INDEX_MASK 0x0000003F +#ifndef PI +#define PI 3.14159265358979f +#endif + + /** + * @brief Macros required for SINE and COSINE Fast math approximations + */ + +#define TABLE_SIZE 256 +#define TABLE_SPACING_Q31 0x800000 +#define TABLE_SPACING_Q15 0x80 + + /** + * @brief Macros required for SINE and COSINE Controller functions + */ + /* 1.31(q31) Fixed value of 2/360 */ + /* -1 to +1 is divided into 360 values so total spacing is (2/360) */ +#define INPUT_SPACING 0xB60B61 + + /** + * @brief Macro for Unaligned Support + */ +#ifndef UNALIGNED_SUPPORT_DISABLE + #define ALIGN4 +#else + #if defined (__GNUC__) + #define ALIGN4 __attribute__((aligned(4))) + #else + #define ALIGN4 __align(4) + #endif +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /** + * @brief Error status returned by some functions in the library. + */ + + typedef enum + { + ARM_MATH_SUCCESS = 0, /**< No error */ + ARM_MATH_ARGUMENT_ERROR = -1, /**< One or more arguments are incorrect */ + ARM_MATH_LENGTH_ERROR = -2, /**< Length of data buffer is incorrect */ + ARM_MATH_SIZE_MISMATCH = -3, /**< Size of matrices is not compatible with the operation. */ + ARM_MATH_NANINF = -4, /**< Not-a-number (NaN) or infinity is generated */ + ARM_MATH_SINGULAR = -5, /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */ + ARM_MATH_TEST_FAILURE = -6 /**< Test Failed */ + } arm_status; + + /** + * @brief 8-bit fractional data type in 1.7 format. + */ + typedef int8_t q7_t; + + /** + * @brief 16-bit fractional data type in 1.15 format. + */ + typedef int16_t q15_t; + + /** + * @brief 32-bit fractional data type in 1.31 format. + */ + typedef int32_t q31_t; + + /** + * @brief 64-bit fractional data type in 1.63 format. + */ + typedef int64_t q63_t; + + /** + * @brief 32-bit floating-point type definition. + */ + typedef float float32_t; + + /** + * @brief 64-bit floating-point type definition. + */ + typedef double float64_t; + + /** + * @brief definition to read/write two 16 bit values. + */ +#if defined __CC_ARM +#define __SIMD32_TYPE int32_t __packed +#define CMSIS_UNUSED __attribute__((unused)) +#elif defined __ICCARM__ +#define CMSIS_UNUSED +#define __SIMD32_TYPE int32_t __packed +#elif defined __GNUC__ +#define __SIMD32_TYPE int32_t +#define CMSIS_UNUSED __attribute__((unused)) +#else +#error Unknown compiler +#endif + +#define __SIMD32(addr) (*(__SIMD32_TYPE **) & (addr)) +#define __SIMD32_CONST(addr) ((__SIMD32_TYPE *)(addr)) + +#define _SIMD32_OFFSET(addr) (*(__SIMD32_TYPE *) (addr)) + +#define __SIMD64(addr) (*(int64_t **) & (addr)) + +#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) + /** + * @brief definition to pack two 16 bit values. + */ +#define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0x0000FFFF) | \ + (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000) ) +#define __PKHTB(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0xFFFF0000) | \ + (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF) ) + +#endif + + + /** + * @brief definition to pack four 8 bit values. + */ +#ifndef ARM_MATH_BIG_ENDIAN + +#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) << 0) & (int32_t)0x000000FF) | \ + (((int32_t)(v1) << 8) & (int32_t)0x0000FF00) | \ + (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \ + (((int32_t)(v3) << 24) & (int32_t)0xFF000000) ) +#else + +#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) << 0) & (int32_t)0x000000FF) | \ + (((int32_t)(v2) << 8) & (int32_t)0x0000FF00) | \ + (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \ + (((int32_t)(v0) << 24) & (int32_t)0xFF000000) ) + +#endif + + + /** + * @brief Clips Q63 to Q31 values. + */ + static __INLINE q31_t clip_q63_to_q31( + q63_t x) + { + return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ? + ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x; + } + + /** + * @brief Clips Q63 to Q15 values. + */ + static __INLINE q15_t clip_q63_to_q15( + q63_t x) + { + return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ? + ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15); + } + + /** + * @brief Clips Q31 to Q7 values. + */ + static __INLINE q7_t clip_q31_to_q7( + q31_t x) + { + return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ? + ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x; + } + + /** + * @brief Clips Q31 to Q15 values. + */ + static __INLINE q15_t clip_q31_to_q15( + q31_t x) + { + return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ? + ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x; + } + + /** + * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format. + */ + + static __INLINE q63_t mult32x64( + q63_t x, + q31_t y) + { + return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) + + (((q63_t) (x >> 32) * y))); + } + + +#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM ) +#define __CLZ __clz +#endif + +#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) ) + + static __INLINE uint32_t __CLZ( + q31_t data); + + + static __INLINE uint32_t __CLZ( + q31_t data) + { + uint32_t count = 0; + uint32_t mask = 0x80000000; + + while((data & mask) == 0) + { + count += 1u; + mask = mask >> 1u; + } + + return (count); + + } + +#endif + + /** + * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type. + */ + + static __INLINE uint32_t arm_recip_q31( + q31_t in, + q31_t * dst, + q31_t * pRecipTable) + { + + uint32_t out, tempVal; + uint32_t index, i; + uint32_t signBits; + + if(in > 0) + { + signBits = __CLZ(in) - 1; + } + else + { + signBits = __CLZ(-in) - 1; + } + + /* Convert input sample to 1.31 format */ + in = in << signBits; + + /* calculation of index for initial approximated Val */ + index = (uint32_t) (in >> 24u); + index = (index & INDEX_MASK); + + /* 1.31 with exp 1 */ + out = pRecipTable[index]; + + /* calculation of reciprocal value */ + /* running approximation for two iterations */ + for (i = 0u; i < 2u; i++) + { + tempVal = (q31_t) (((q63_t) in * out) >> 31u); + tempVal = 0x7FFFFFFF - tempVal; + /* 1.31 with exp 1 */ + //out = (q31_t) (((q63_t) out * tempVal) >> 30u); + out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u); + } + + /* write output */ + *dst = out; + + /* return num of signbits of out = 1/in value */ + return (signBits + 1u); + + } + + /** + * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type. + */ + static __INLINE uint32_t arm_recip_q15( + q15_t in, + q15_t * dst, + q15_t * pRecipTable) + { + + uint32_t out = 0, tempVal = 0; + uint32_t index = 0, i = 0; + uint32_t signBits = 0; + + if(in > 0) + { + signBits = __CLZ(in) - 17; + } + else + { + signBits = __CLZ(-in) - 17; + } + + /* Convert input sample to 1.15 format */ + in = in << signBits; + + /* calculation of index for initial approximated Val */ + index = in >> 8; + index = (index & INDEX_MASK); + + /* 1.15 with exp 1 */ + out = pRecipTable[index]; + + /* calculation of reciprocal value */ + /* running approximation for two iterations */ + for (i = 0; i < 2; i++) + { + tempVal = (q15_t) (((q31_t) in * out) >> 15); + tempVal = 0x7FFF - tempVal; + /* 1.15 with exp 1 */ + out = (q15_t) (((q31_t) out * tempVal) >> 14); + } + + /* write output */ + *dst = out; + + /* return num of signbits of out = 1/in value */ + return (signBits + 1); + + } + + + /* + * @brief C custom defined intrinisic function for only M0 processors + */ +#if defined(ARM_MATH_CM0_FAMILY) + + static __INLINE q31_t __SSAT( + q31_t x, + uint32_t y) + { + int32_t posMax, negMin; + uint32_t i; + + posMax = 1; + for (i = 0; i < (y - 1); i++) + { + posMax = posMax * 2; + } + + if(x > 0) + { + posMax = (posMax - 1); + + if(x > posMax) + { + x = posMax; + } + } + else + { + negMin = -posMax; + + if(x < negMin) + { + x = negMin; + } + } + return (x); + + + } + +#endif /* end of ARM_MATH_CM0_FAMILY */ + + + + /* + * @brief C custom defined intrinsic function for M3 and M0 processors + */ +#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) + + /* + * @brief C custom defined QADD8 for M3 and M0 processors + */ + static __INLINE q31_t __QADD8( + q31_t x, + q31_t y) + { + + q31_t sum; + q7_t r, s, t, u; + + r = (q7_t) x; + s = (q7_t) y; + + r = __SSAT((q31_t) (r + s), 8); + s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8); + t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8); + u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8); + + sum = + (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) | + (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF); + + return sum; + + } + + /* + * @brief C custom defined QSUB8 for M3 and M0 processors + */ + static __INLINE q31_t __QSUB8( + q31_t x, + q31_t y) + { + + q31_t sum; + q31_t r, s, t, u; + + r = (q7_t) x; + s = (q7_t) y; + + r = __SSAT((r - s), 8); + s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8; + t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16; + u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24; + + sum = + (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r & + 0x000000FF); + + return sum; + } + + /* + * @brief C custom defined QADD16 for M3 and M0 processors + */ + + /* + * @brief C custom defined QADD16 for M3 and M0 processors + */ + static __INLINE q31_t __QADD16( + q31_t x, + q31_t y) + { + + q31_t sum; + q31_t r, s; + + r = (short) x; + s = (short) y; + + r = __SSAT(r + s, 16); + s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16; + + sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); + + return sum; + + } + + /* + * @brief C custom defined SHADD16 for M3 and M0 processors + */ + static __INLINE q31_t __SHADD16( + q31_t x, + q31_t y) + { + + q31_t sum; + q31_t r, s; + + r = (short) x; + s = (short) y; + + r = ((r >> 1) + (s >> 1)); + s = ((q31_t) ((x >> 17) + (y >> 17))) << 16; + + sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); + + return sum; + + } + + /* + * @brief C custom defined QSUB16 for M3 and M0 processors + */ + static __INLINE q31_t __QSUB16( + q31_t x, + q31_t y) + { + + q31_t sum; + q31_t r, s; + + r = (short) x; + s = (short) y; + + r = __SSAT(r - s, 16); + s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16; + + sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); + + return sum; + } + + /* + * @brief C custom defined SHSUB16 for M3 and M0 processors + */ + static __INLINE q31_t __SHSUB16( + q31_t x, + q31_t y) + { + + q31_t diff; + q31_t r, s; + + r = (short) x; + s = (short) y; + + r = ((r >> 1) - (s >> 1)); + s = (((x >> 17) - (y >> 17)) << 16); + + diff = (s & 0xFFFF0000) | (r & 0x0000FFFF); + + return diff; + } + + /* + * @brief C custom defined QASX for M3 and M0 processors + */ + static __INLINE q31_t __QASX( + q31_t x, + q31_t y) + { + + q31_t sum = 0; + + sum = + ((sum + + clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) + + clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16))); + + return sum; + } + + /* + * @brief C custom defined SHASX for M3 and M0 processors + */ + static __INLINE q31_t __SHASX( + q31_t x, + q31_t y) + { + + q31_t sum; + q31_t r, s; + + r = (short) x; + s = (short) y; + + r = ((r >> 1) - (y >> 17)); + s = (((x >> 17) + (s >> 1)) << 16); + + sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); + + return sum; + } + + + /* + * @brief C custom defined QSAX for M3 and M0 processors + */ + static __INLINE q31_t __QSAX( + q31_t x, + q31_t y) + { + + q31_t sum = 0; + + sum = + ((sum + + clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) + + clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16))); + + return sum; + } + + /* + * @brief C custom defined SHSAX for M3 and M0 processors + */ + static __INLINE q31_t __SHSAX( + q31_t x, + q31_t y) + { + + q31_t sum; + q31_t r, s; + + r = (short) x; + s = (short) y; + + r = ((r >> 1) + (y >> 17)); + s = (((x >> 17) - (s >> 1)) << 16); + + sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); + + return sum; + } + + /* + * @brief C custom defined SMUSDX for M3 and M0 processors + */ + static __INLINE q31_t __SMUSDX( + q31_t x, + q31_t y) + { + + return ((q31_t) (((short) x * (short) (y >> 16)) - + ((short) (x >> 16) * (short) y))); + } + + /* + * @brief C custom defined SMUADX for M3 and M0 processors + */ + static __INLINE q31_t __SMUADX( + q31_t x, + q31_t y) + { + + return ((q31_t) (((short) x * (short) (y >> 16)) + + ((short) (x >> 16) * (short) y))); + } + + /* + * @brief C custom defined QADD for M3 and M0 processors + */ + static __INLINE q31_t __QADD( + q31_t x, + q31_t y) + { + return clip_q63_to_q31((q63_t) x + y); + } + + /* + * @brief C custom defined QSUB for M3 and M0 processors + */ + static __INLINE q31_t __QSUB( + q31_t x, + q31_t y) + { + return clip_q63_to_q31((q63_t) x - y); + } + + /* + * @brief C custom defined SMLAD for M3 and M0 processors + */ + static __INLINE q31_t __SMLAD( + q31_t x, + q31_t y, + q31_t sum) + { + + return (sum + ((short) (x >> 16) * (short) (y >> 16)) + + ((short) x * (short) y)); + } + + /* + * @brief C custom defined SMLADX for M3 and M0 processors + */ + static __INLINE q31_t __SMLADX( + q31_t x, + q31_t y, + q31_t sum) + { + + return (sum + ((short) (x >> 16) * (short) (y)) + + ((short) x * (short) (y >> 16))); + } + + /* + * @brief C custom defined SMLSDX for M3 and M0 processors + */ + static __INLINE q31_t __SMLSDX( + q31_t x, + q31_t y, + q31_t sum) + { + + return (sum - ((short) (x >> 16) * (short) (y)) + + ((short) x * (short) (y >> 16))); + } + + /* + * @brief C custom defined SMLALD for M3 and M0 processors + */ + static __INLINE q63_t __SMLALD( + q31_t x, + q31_t y, + q63_t sum) + { + + return (sum + ((short) (x >> 16) * (short) (y >> 16)) + + ((short) x * (short) y)); + } + + /* + * @brief C custom defined SMLALDX for M3 and M0 processors + */ + static __INLINE q63_t __SMLALDX( + q31_t x, + q31_t y, + q63_t sum) + { + + return (sum + ((short) (x >> 16) * (short) y)) + + ((short) x * (short) (y >> 16)); + } + + /* + * @brief C custom defined SMUAD for M3 and M0 processors + */ + static __INLINE q31_t __SMUAD( + q31_t x, + q31_t y) + { + + return (((x >> 16) * (y >> 16)) + + (((x << 16) >> 16) * ((y << 16) >> 16))); + } + + /* + * @brief C custom defined SMUSD for M3 and M0 processors + */ + static __INLINE q31_t __SMUSD( + q31_t x, + q31_t y) + { + + return (-((x >> 16) * (y >> 16)) + + (((x << 16) >> 16) * ((y << 16) >> 16))); + } + + + /* + * @brief C custom defined SXTB16 for M3 and M0 processors + */ + static __INLINE q31_t __SXTB16( + q31_t x) + { + + return ((((x << 24) >> 24) & 0x0000FFFF) | + (((x << 8) >> 8) & 0xFFFF0000)); + } + + +#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */ + + + /** + * @brief Instance structure for the Q7 FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + q7_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + } arm_fir_instance_q7; + + /** + * @brief Instance structure for the Q15 FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + } arm_fir_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + } arm_fir_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of filter coefficients in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + } arm_fir_instance_f32; + + + /** + * @brief Processing function for the Q7 FIR filter. + * @param[in] *S points to an instance of the Q7 FIR filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_q7( + const arm_fir_instance_q7 * S, + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q7 FIR filter. + * @param[in,out] *S points to an instance of the Q7 FIR structure. + * @param[in] numTaps Number of filter coefficients in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of samples that are processed. + * @return none + */ + void arm_fir_init_q7( + arm_fir_instance_q7 * S, + uint16_t numTaps, + q7_t * pCoeffs, + q7_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 FIR filter. + * @param[in] *S points to an instance of the Q15 FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_q15( + const arm_fir_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4. + * @param[in] *S points to an instance of the Q15 FIR filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_fast_q15( + const arm_fir_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q15 FIR filter. + * @param[in,out] *S points to an instance of the Q15 FIR filter structure. + * @param[in] numTaps Number of filter coefficients in the filter. Must be even and greater than or equal to 4. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of samples that are processed at a time. + * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if + * numTaps is not a supported value. + */ + + arm_status arm_fir_init_q15( + arm_fir_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + uint32_t blockSize); + + /** + * @brief Processing function for the Q31 FIR filter. + * @param[in] *S points to an instance of the Q31 FIR filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_q31( + const arm_fir_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4. + * @param[in] *S points to an instance of the Q31 FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_fast_q31( + const arm_fir_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q31 FIR filter. + * @param[in,out] *S points to an instance of the Q31 FIR structure. + * @param[in] numTaps Number of filter coefficients in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of samples that are processed at a time. + * @return none. + */ + void arm_fir_init_q31( + arm_fir_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + uint32_t blockSize); + + /** + * @brief Processing function for the floating-point FIR filter. + * @param[in] *S points to an instance of the floating-point FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_f32( + const arm_fir_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the floating-point FIR filter. + * @param[in,out] *S points to an instance of the floating-point FIR filter structure. + * @param[in] numTaps Number of filter coefficients in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of samples that are processed at a time. + * @return none. + */ + void arm_fir_init_f32( + arm_fir_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q15 Biquad cascade filter. + */ + typedef struct + { + int8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + q15_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + q15_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + int8_t postShift; /**< Additional shift, in bits, applied to each output sample. */ + + } arm_biquad_casd_df1_inst_q15; + + + /** + * @brief Instance structure for the Q31 Biquad cascade filter. + */ + typedef struct + { + uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + q31_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + q31_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + uint8_t postShift; /**< Additional shift, in bits, applied to each output sample. */ + + } arm_biquad_casd_df1_inst_q31; + + /** + * @brief Instance structure for the floating-point Biquad cascade filter. + */ + typedef struct + { + uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float32_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + float32_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + + + } arm_biquad_casd_df1_inst_f32; + + + + /** + * @brief Processing function for the Q15 Biquad cascade filter. + * @param[in] *S points to an instance of the Q15 Biquad cascade structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cascade_df1_q15( + const arm_biquad_casd_df1_inst_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q15 Biquad cascade filter. + * @param[in,out] *S points to an instance of the Q15 Biquad cascade structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format + * @return none + */ + + void arm_biquad_cascade_df1_init_q15( + arm_biquad_casd_df1_inst_q15 * S, + uint8_t numStages, + q15_t * pCoeffs, + q15_t * pState, + int8_t postShift); + + + /** + * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4. + * @param[in] *S points to an instance of the Q15 Biquad cascade structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cascade_df1_fast_q15( + const arm_biquad_casd_df1_inst_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 Biquad cascade filter + * @param[in] *S points to an instance of the Q31 Biquad cascade structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cascade_df1_q31( + const arm_biquad_casd_df1_inst_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4. + * @param[in] *S points to an instance of the Q31 Biquad cascade structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cascade_df1_fast_q31( + const arm_biquad_casd_df1_inst_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q31 Biquad cascade filter. + * @param[in,out] *S points to an instance of the Q31 Biquad cascade structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format + * @return none + */ + + void arm_biquad_cascade_df1_init_q31( + arm_biquad_casd_df1_inst_q31 * S, + uint8_t numStages, + q31_t * pCoeffs, + q31_t * pState, + int8_t postShift); + + /** + * @brief Processing function for the floating-point Biquad cascade filter. + * @param[in] *S points to an instance of the floating-point Biquad cascade structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cascade_df1_f32( + const arm_biquad_casd_df1_inst_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the floating-point Biquad cascade filter. + * @param[in,out] *S points to an instance of the floating-point Biquad cascade structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @return none + */ + + void arm_biquad_cascade_df1_init_f32( + arm_biquad_casd_df1_inst_f32 * S, + uint8_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + + /** + * @brief Instance structure for the floating-point matrix structure. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + float32_t *pData; /**< points to the data of the matrix. */ + } arm_matrix_instance_f32; + + /** + * @brief Instance structure for the Q15 matrix structure. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + q15_t *pData; /**< points to the data of the matrix. */ + + } arm_matrix_instance_q15; + + /** + * @brief Instance structure for the Q31 matrix structure. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows of the matrix. */ + uint16_t numCols; /**< number of columns of the matrix. */ + q31_t *pData; /**< points to the data of the matrix. */ + + } arm_matrix_instance_q31; + + + + /** + * @brief Floating-point matrix addition. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_add_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + /** + * @brief Q15 matrix addition. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_add_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst); + + /** + * @brief Q31 matrix addition. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_add_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix transpose. + * @param[in] *pSrc points to the input matrix + * @param[out] *pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_trans_f32( + const arm_matrix_instance_f32 * pSrc, + arm_matrix_instance_f32 * pDst); + + + /** + * @brief Q15 matrix transpose. + * @param[in] *pSrc points to the input matrix + * @param[out] *pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_trans_q15( + const arm_matrix_instance_q15 * pSrc, + arm_matrix_instance_q15 * pDst); + + /** + * @brief Q31 matrix transpose. + * @param[in] *pSrc points to the input matrix + * @param[out] *pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_trans_q31( + const arm_matrix_instance_q31 * pSrc, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix multiplication + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + /** + * @brief Q15 matrix multiplication + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @param[in] *pState points to the array for storing intermediate results + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pState); + + /** + * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @param[in] *pState points to the array for storing intermediate results + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_mult_fast_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pState); + + /** + * @brief Q31 matrix multiplication + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + /** + * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_mult_fast_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Floating-point matrix subtraction + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_sub_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst); + + /** + * @brief Q15 matrix subtraction + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_sub_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst); + + /** + * @brief Q31 matrix subtraction + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_sub_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst); + + /** + * @brief Floating-point matrix scaling. + * @param[in] *pSrc points to the input matrix + * @param[in] scale scale factor + * @param[out] *pDst points to the output matrix + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_scale_f32( + const arm_matrix_instance_f32 * pSrc, + float32_t scale, + arm_matrix_instance_f32 * pDst); + + /** + * @brief Q15 matrix scaling. + * @param[in] *pSrc points to input matrix + * @param[in] scaleFract fractional portion of the scale factor + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to output matrix + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_scale_q15( + const arm_matrix_instance_q15 * pSrc, + q15_t scaleFract, + int32_t shift, + arm_matrix_instance_q15 * pDst); + + /** + * @brief Q31 matrix scaling. + * @param[in] *pSrc points to input matrix + * @param[in] scaleFract fractional portion of the scale factor + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + arm_status arm_mat_scale_q31( + const arm_matrix_instance_q31 * pSrc, + q31_t scaleFract, + int32_t shift, + arm_matrix_instance_q31 * pDst); + + + /** + * @brief Q31 matrix initialization. + * @param[in,out] *S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] *pData points to the matrix data array. + * @return none + */ + + void arm_mat_init_q31( + arm_matrix_instance_q31 * S, + uint16_t nRows, + uint16_t nColumns, + q31_t * pData); + + /** + * @brief Q15 matrix initialization. + * @param[in,out] *S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] *pData points to the matrix data array. + * @return none + */ + + void arm_mat_init_q15( + arm_matrix_instance_q15 * S, + uint16_t nRows, + uint16_t nColumns, + q15_t * pData); + + /** + * @brief Floating-point matrix initialization. + * @param[in,out] *S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] *pData points to the matrix data array. + * @return none + */ + + void arm_mat_init_f32( + arm_matrix_instance_f32 * S, + uint16_t nRows, + uint16_t nColumns, + float32_t * pData); + + + + /** + * @brief Instance structure for the Q15 PID Control. + */ + typedef struct + { + q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ +#ifdef ARM_MATH_CM0_FAMILY + q15_t A1; + q15_t A2; +#else + q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/ +#endif + q15_t state[3]; /**< The state array of length 3. */ + q15_t Kp; /**< The proportional gain. */ + q15_t Ki; /**< The integral gain. */ + q15_t Kd; /**< The derivative gain. */ + } arm_pid_instance_q15; + + /** + * @brief Instance structure for the Q31 PID Control. + */ + typedef struct + { + q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ + q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ + q31_t A2; /**< The derived gain, A2 = Kd . */ + q31_t state[3]; /**< The state array of length 3. */ + q31_t Kp; /**< The proportional gain. */ + q31_t Ki; /**< The integral gain. */ + q31_t Kd; /**< The derivative gain. */ + + } arm_pid_instance_q31; + + /** + * @brief Instance structure for the floating-point PID Control. + */ + typedef struct + { + float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ + float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ + float32_t A2; /**< The derived gain, A2 = Kd . */ + float32_t state[3]; /**< The state array of length 3. */ + float32_t Kp; /**< The proportional gain. */ + float32_t Ki; /**< The integral gain. */ + float32_t Kd; /**< The derivative gain. */ + } arm_pid_instance_f32; + + + + /** + * @brief Initialization function for the floating-point PID Control. + * @param[in,out] *S points to an instance of the PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + * @return none. + */ + void arm_pid_init_f32( + arm_pid_instance_f32 * S, + int32_t resetStateFlag); + + /** + * @brief Reset function for the floating-point PID Control. + * @param[in,out] *S is an instance of the floating-point PID Control structure + * @return none + */ + void arm_pid_reset_f32( + arm_pid_instance_f32 * S); + + + /** + * @brief Initialization function for the Q31 PID Control. + * @param[in,out] *S points to an instance of the Q15 PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + * @return none. + */ + void arm_pid_init_q31( + arm_pid_instance_q31 * S, + int32_t resetStateFlag); + + + /** + * @brief Reset function for the Q31 PID Control. + * @param[in,out] *S points to an instance of the Q31 PID Control structure + * @return none + */ + + void arm_pid_reset_q31( + arm_pid_instance_q31 * S); + + /** + * @brief Initialization function for the Q15 PID Control. + * @param[in,out] *S points to an instance of the Q15 PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + * @return none. + */ + void arm_pid_init_q15( + arm_pid_instance_q15 * S, + int32_t resetStateFlag); + + /** + * @brief Reset function for the Q15 PID Control. + * @param[in,out] *S points to an instance of the q15 PID Control structure + * @return none + */ + void arm_pid_reset_q15( + arm_pid_instance_q15 * S); + + + /** + * @brief Instance structure for the floating-point Linear Interpolate function. + */ + typedef struct + { + uint32_t nValues; /**< nValues */ + float32_t x1; /**< x1 */ + float32_t xSpacing; /**< xSpacing */ + float32_t *pYData; /**< pointer to the table of Y values */ + } arm_linear_interp_instance_f32; + + /** + * @brief Instance structure for the floating-point bilinear interpolation function. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + float32_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_f32; + + /** + * @brief Instance structure for the Q31 bilinear interpolation function. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + q31_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_q31; + + /** + * @brief Instance structure for the Q15 bilinear interpolation function. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + q15_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_q15; + + /** + * @brief Instance structure for the Q15 bilinear interpolation function. + */ + + typedef struct + { + uint16_t numRows; /**< number of rows in the data table. */ + uint16_t numCols; /**< number of columns in the data table. */ + q7_t *pData; /**< points to the data table. */ + } arm_bilinear_interp_instance_q7; + + + /** + * @brief Q7 vector multiplication. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_mult_q7( + q7_t * pSrcA, + q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Q15 vector multiplication. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_mult_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Q31 vector multiplication. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_mult_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Floating-point vector multiplication. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_mult_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize); + + + + + + + /** + * @brief Instance structure for the Q15 CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q15_t *pTwiddle; /**< points to the Sin twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix2_instance_q15; + + arm_status arm_cfft_radix2_init_q15( + arm_cfft_radix2_instance_q15 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + void arm_cfft_radix2_q15( + const arm_cfft_radix2_instance_q15 * S, + q15_t * pSrc); + + + + /** + * @brief Instance structure for the Q15 CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q15_t *pTwiddle; /**< points to the twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix4_instance_q15; + + arm_status arm_cfft_radix4_init_q15( + arm_cfft_radix4_instance_q15 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + void arm_cfft_radix4_q15( + const arm_cfft_radix4_instance_q15 * S, + q15_t * pSrc); + + /** + * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q31_t *pTwiddle; /**< points to the Twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix2_instance_q31; + + arm_status arm_cfft_radix2_init_q31( + arm_cfft_radix2_instance_q31 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + void arm_cfft_radix2_q31( + const arm_cfft_radix2_instance_q31 * S, + q31_t * pSrc); + + /** + * @brief Instance structure for the Q31 CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + q31_t *pTwiddle; /**< points to the twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + } arm_cfft_radix4_instance_q31; + + + void arm_cfft_radix4_q31( + const arm_cfft_radix4_instance_q31 * S, + q31_t * pSrc); + + arm_status arm_cfft_radix4_init_q31( + arm_cfft_radix4_instance_q31 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + /** + * @brief Instance structure for the floating-point CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + float32_t *pTwiddle; /**< points to the Twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + float32_t onebyfftLen; /**< value of 1/fftLen. */ + } arm_cfft_radix2_instance_f32; + +/* Deprecated */ + arm_status arm_cfft_radix2_init_f32( + arm_cfft_radix2_instance_f32 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix2_f32( + const arm_cfft_radix2_instance_f32 * S, + float32_t * pSrc); + + /** + * @brief Instance structure for the floating-point CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ + uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ + float32_t *pTwiddle; /**< points to the Twiddle factor table. */ + uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ + float32_t onebyfftLen; /**< value of 1/fftLen. */ + } arm_cfft_radix4_instance_f32; + +/* Deprecated */ + arm_status arm_cfft_radix4_init_f32( + arm_cfft_radix4_instance_f32 * S, + uint16_t fftLen, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + +/* Deprecated */ + void arm_cfft_radix4_f32( + const arm_cfft_radix4_instance_f32 * S, + float32_t * pSrc); + + /** + * @brief Instance structure for the floating-point CFFT/CIFFT function. + */ + + typedef struct + { + uint16_t fftLen; /**< length of the FFT. */ + const float32_t *pTwiddle; /**< points to the Twiddle factor table. */ + const uint16_t *pBitRevTable; /**< points to the bit reversal table. */ + uint16_t bitRevLength; /**< bit reversal table length. */ + } arm_cfft_instance_f32; + + void arm_cfft_f32( + const arm_cfft_instance_f32 * S, + float32_t * p1, + uint8_t ifftFlag, + uint8_t bitReverseFlag); + + /** + * @brief Instance structure for the Q15 RFFT/RIFFT function. + */ + + typedef struct + { + uint32_t fftLenReal; /**< length of the real FFT. */ + uint32_t fftLenBy2; /**< length of the complex FFT. */ + uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ + uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ + uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + q15_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ + q15_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ + arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */ + } arm_rfft_instance_q15; + + arm_status arm_rfft_init_q15( + arm_rfft_instance_q15 * S, + arm_cfft_radix4_instance_q15 * S_CFFT, + uint32_t fftLenReal, + uint32_t ifftFlagR, + uint32_t bitReverseFlag); + + void arm_rfft_q15( + const arm_rfft_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst); + + /** + * @brief Instance structure for the Q31 RFFT/RIFFT function. + */ + + typedef struct + { + uint32_t fftLenReal; /**< length of the real FFT. */ + uint32_t fftLenBy2; /**< length of the complex FFT. */ + uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ + uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ + uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + q31_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ + q31_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ + arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */ + } arm_rfft_instance_q31; + + arm_status arm_rfft_init_q31( + arm_rfft_instance_q31 * S, + arm_cfft_radix4_instance_q31 * S_CFFT, + uint32_t fftLenReal, + uint32_t ifftFlagR, + uint32_t bitReverseFlag); + + void arm_rfft_q31( + const arm_rfft_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst); + + /** + * @brief Instance structure for the floating-point RFFT/RIFFT function. + */ + + typedef struct + { + uint32_t fftLenReal; /**< length of the real FFT. */ + uint16_t fftLenBy2; /**< length of the complex FFT. */ + uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ + uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ + uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ + float32_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ + float32_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ + arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */ + } arm_rfft_instance_f32; + + arm_status arm_rfft_init_f32( + arm_rfft_instance_f32 * S, + arm_cfft_radix4_instance_f32 * S_CFFT, + uint32_t fftLenReal, + uint32_t ifftFlagR, + uint32_t bitReverseFlag); + + void arm_rfft_f32( + const arm_rfft_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst); + + /** + * @brief Instance structure for the floating-point RFFT/RIFFT function. + */ + +typedef struct + { + arm_cfft_instance_f32 Sint; /**< Internal CFFT structure. */ + uint16_t fftLenRFFT; /**< length of the real sequence */ + float32_t * pTwiddleRFFT; /**< Twiddle factors real stage */ + } arm_rfft_fast_instance_f32 ; + +arm_status arm_rfft_fast_init_f32 ( + arm_rfft_fast_instance_f32 * S, + uint16_t fftLen); + +void arm_rfft_fast_f32( + arm_rfft_fast_instance_f32 * S, + float32_t * p, float32_t * pOut, + uint8_t ifftFlag); + + /** + * @brief Instance structure for the floating-point DCT4/IDCT4 function. + */ + + typedef struct + { + uint16_t N; /**< length of the DCT4. */ + uint16_t Nby2; /**< half of the length of the DCT4. */ + float32_t normalize; /**< normalizing factor. */ + float32_t *pTwiddle; /**< points to the twiddle factor table. */ + float32_t *pCosFactor; /**< points to the cosFactor table. */ + arm_rfft_instance_f32 *pRfft; /**< points to the real FFT instance. */ + arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */ + } arm_dct4_instance_f32; + + /** + * @brief Initialization function for the floating-point DCT4/IDCT4. + * @param[in,out] *S points to an instance of floating-point DCT4/IDCT4 structure. + * @param[in] *S_RFFT points to an instance of floating-point RFFT/RIFFT structure. + * @param[in] *S_CFFT points to an instance of floating-point CFFT/CIFFT structure. + * @param[in] N length of the DCT4. + * @param[in] Nby2 half of the length of the DCT4. + * @param[in] normalize normalizing factor. + * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported transform length. + */ + + arm_status arm_dct4_init_f32( + arm_dct4_instance_f32 * S, + arm_rfft_instance_f32 * S_RFFT, + arm_cfft_radix4_instance_f32 * S_CFFT, + uint16_t N, + uint16_t Nby2, + float32_t normalize); + + /** + * @brief Processing function for the floating-point DCT4/IDCT4. + * @param[in] *S points to an instance of the floating-point DCT4/IDCT4 structure. + * @param[in] *pState points to state buffer. + * @param[in,out] *pInlineBuffer points to the in-place input and output buffer. + * @return none. + */ + + void arm_dct4_f32( + const arm_dct4_instance_f32 * S, + float32_t * pState, + float32_t * pInlineBuffer); + + /** + * @brief Instance structure for the Q31 DCT4/IDCT4 function. + */ + + typedef struct + { + uint16_t N; /**< length of the DCT4. */ + uint16_t Nby2; /**< half of the length of the DCT4. */ + q31_t normalize; /**< normalizing factor. */ + q31_t *pTwiddle; /**< points to the twiddle factor table. */ + q31_t *pCosFactor; /**< points to the cosFactor table. */ + arm_rfft_instance_q31 *pRfft; /**< points to the real FFT instance. */ + arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */ + } arm_dct4_instance_q31; + + /** + * @brief Initialization function for the Q31 DCT4/IDCT4. + * @param[in,out] *S points to an instance of Q31 DCT4/IDCT4 structure. + * @param[in] *S_RFFT points to an instance of Q31 RFFT/RIFFT structure + * @param[in] *S_CFFT points to an instance of Q31 CFFT/CIFFT structure + * @param[in] N length of the DCT4. + * @param[in] Nby2 half of the length of the DCT4. + * @param[in] normalize normalizing factor. + * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length. + */ + + arm_status arm_dct4_init_q31( + arm_dct4_instance_q31 * S, + arm_rfft_instance_q31 * S_RFFT, + arm_cfft_radix4_instance_q31 * S_CFFT, + uint16_t N, + uint16_t Nby2, + q31_t normalize); + + /** + * @brief Processing function for the Q31 DCT4/IDCT4. + * @param[in] *S points to an instance of the Q31 DCT4 structure. + * @param[in] *pState points to state buffer. + * @param[in,out] *pInlineBuffer points to the in-place input and output buffer. + * @return none. + */ + + void arm_dct4_q31( + const arm_dct4_instance_q31 * S, + q31_t * pState, + q31_t * pInlineBuffer); + + /** + * @brief Instance structure for the Q15 DCT4/IDCT4 function. + */ + + typedef struct + { + uint16_t N; /**< length of the DCT4. */ + uint16_t Nby2; /**< half of the length of the DCT4. */ + q15_t normalize; /**< normalizing factor. */ + q15_t *pTwiddle; /**< points to the twiddle factor table. */ + q15_t *pCosFactor; /**< points to the cosFactor table. */ + arm_rfft_instance_q15 *pRfft; /**< points to the real FFT instance. */ + arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */ + } arm_dct4_instance_q15; + + /** + * @brief Initialization function for the Q15 DCT4/IDCT4. + * @param[in,out] *S points to an instance of Q15 DCT4/IDCT4 structure. + * @param[in] *S_RFFT points to an instance of Q15 RFFT/RIFFT structure. + * @param[in] *S_CFFT points to an instance of Q15 CFFT/CIFFT structure. + * @param[in] N length of the DCT4. + * @param[in] Nby2 half of the length of the DCT4. + * @param[in] normalize normalizing factor. + * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length. + */ + + arm_status arm_dct4_init_q15( + arm_dct4_instance_q15 * S, + arm_rfft_instance_q15 * S_RFFT, + arm_cfft_radix4_instance_q15 * S_CFFT, + uint16_t N, + uint16_t Nby2, + q15_t normalize); + + /** + * @brief Processing function for the Q15 DCT4/IDCT4. + * @param[in] *S points to an instance of the Q15 DCT4 structure. + * @param[in] *pState points to state buffer. + * @param[in,out] *pInlineBuffer points to the in-place input and output buffer. + * @return none. + */ + + void arm_dct4_q15( + const arm_dct4_instance_q15 * S, + q15_t * pState, + q15_t * pInlineBuffer); + + /** + * @brief Floating-point vector addition. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_add_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Q7 vector addition. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_add_q7( + q7_t * pSrcA, + q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Q15 vector addition. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_add_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Q31 vector addition. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_add_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Floating-point vector subtraction. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_sub_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Q7 vector subtraction. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_sub_q7( + q7_t * pSrcA, + q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Q15 vector subtraction. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_sub_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Q31 vector subtraction. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_sub_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Multiplies a floating-point vector by a scalar. + * @param[in] *pSrc points to the input vector + * @param[in] scale scale factor to be applied + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_scale_f32( + float32_t * pSrc, + float32_t scale, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Multiplies a Q7 vector by a scalar. + * @param[in] *pSrc points to the input vector + * @param[in] scaleFract fractional portion of the scale value + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_scale_q7( + q7_t * pSrc, + q7_t scaleFract, + int8_t shift, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Multiplies a Q15 vector by a scalar. + * @param[in] *pSrc points to the input vector + * @param[in] scaleFract fractional portion of the scale value + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_scale_q15( + q15_t * pSrc, + q15_t scaleFract, + int8_t shift, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Multiplies a Q31 vector by a scalar. + * @param[in] *pSrc points to the input vector + * @param[in] scaleFract fractional portion of the scale value + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_scale_q31( + q31_t * pSrc, + q31_t scaleFract, + int8_t shift, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Q7 vector absolute value. + * @param[in] *pSrc points to the input buffer + * @param[out] *pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_abs_q7( + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Floating-point vector absolute value. + * @param[in] *pSrc points to the input buffer + * @param[out] *pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_abs_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Q15 vector absolute value. + * @param[in] *pSrc points to the input buffer + * @param[out] *pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_abs_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Q31 vector absolute value. + * @param[in] *pSrc points to the input buffer + * @param[out] *pDst points to the output buffer + * @param[in] blockSize number of samples in each vector + * @return none. + */ + + void arm_abs_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Dot product of floating-point vectors. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] *result output result returned here + * @return none. + */ + + void arm_dot_prod_f32( + float32_t * pSrcA, + float32_t * pSrcB, + uint32_t blockSize, + float32_t * result); + + /** + * @brief Dot product of Q7 vectors. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] *result output result returned here + * @return none. + */ + + void arm_dot_prod_q7( + q7_t * pSrcA, + q7_t * pSrcB, + uint32_t blockSize, + q31_t * result); + + /** + * @brief Dot product of Q15 vectors. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] *result output result returned here + * @return none. + */ + + void arm_dot_prod_q15( + q15_t * pSrcA, + q15_t * pSrcB, + uint32_t blockSize, + q63_t * result); + + /** + * @brief Dot product of Q31 vectors. + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] blockSize number of samples in each vector + * @param[out] *result output result returned here + * @return none. + */ + + void arm_dot_prod_q31( + q31_t * pSrcA, + q31_t * pSrcB, + uint32_t blockSize, + q63_t * result); + + /** + * @brief Shifts the elements of a Q7 vector a specified number of bits. + * @param[in] *pSrc points to the input vector + * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_shift_q7( + q7_t * pSrc, + int8_t shiftBits, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Shifts the elements of a Q15 vector a specified number of bits. + * @param[in] *pSrc points to the input vector + * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_shift_q15( + q15_t * pSrc, + int8_t shiftBits, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Shifts the elements of a Q31 vector a specified number of bits. + * @param[in] *pSrc points to the input vector + * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_shift_q31( + q31_t * pSrc, + int8_t shiftBits, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Adds a constant offset to a floating-point vector. + * @param[in] *pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_offset_f32( + float32_t * pSrc, + float32_t offset, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Adds a constant offset to a Q7 vector. + * @param[in] *pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_offset_q7( + q7_t * pSrc, + q7_t offset, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Adds a constant offset to a Q15 vector. + * @param[in] *pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_offset_q15( + q15_t * pSrc, + q15_t offset, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Adds a constant offset to a Q31 vector. + * @param[in] *pSrc points to the input vector + * @param[in] offset is the offset to be added + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_offset_q31( + q31_t * pSrc, + q31_t offset, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Negates the elements of a floating-point vector. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_negate_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Negates the elements of a Q7 vector. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_negate_q7( + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Negates the elements of a Q15 vector. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_negate_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Negates the elements of a Q31 vector. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] blockSize number of samples in the vector + * @return none. + */ + + void arm_negate_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + /** + * @brief Copies the elements of a floating-point vector. + * @param[in] *pSrc input pointer + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_copy_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Copies the elements of a Q7 vector. + * @param[in] *pSrc input pointer + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_copy_q7( + q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Copies the elements of a Q15 vector. + * @param[in] *pSrc input pointer + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_copy_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Copies the elements of a Q31 vector. + * @param[in] *pSrc input pointer + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_copy_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + /** + * @brief Fills a constant value into a floating-point vector. + * @param[in] value input value to be filled + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_fill_f32( + float32_t value, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Fills a constant value into a Q7 vector. + * @param[in] value input value to be filled + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_fill_q7( + q7_t value, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Fills a constant value into a Q15 vector. + * @param[in] value input value to be filled + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_fill_q15( + q15_t value, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Fills a constant value into a Q31 vector. + * @param[in] value input value to be filled + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_fill_q31( + q31_t value, + q31_t * pDst, + uint32_t blockSize); + +/** + * @brief Convolution of floating-point sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1. + * @return none. + */ + + void arm_conv_f32( + float32_t * pSrcA, + uint32_t srcALen, + float32_t * pSrcB, + uint32_t srcBLen, + float32_t * pDst); + + + /** + * @brief Convolution of Q15 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + * @return none. + */ + + + void arm_conv_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + +/** + * @brief Convolution of Q15 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1. + * @return none. + */ + + void arm_conv_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + /** + * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @return none. + */ + + void arm_conv_fast_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + /** + * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + * @return none. + */ + + void arm_conv_fast_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + + + /** + * @brief Convolution of Q31 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @return none. + */ + + void arm_conv_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + /** + * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @return none. + */ + + void arm_conv_fast_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + + /** + * @brief Convolution of Q7 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). + * @return none. + */ + + void arm_conv_opt_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + + + /** + * @brief Convolution of Q7 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. + * @return none. + */ + + void arm_conv_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst); + + + /** + * @brief Partial convolution of floating-point sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_f32( + float32_t * pSrcA, + uint32_t srcALen, + float32_t * pSrcB, + uint32_t srcBLen, + float32_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + /** + * @brief Partial convolution of Q15 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints, + q15_t * pScratch1, + q15_t * pScratch2); + + +/** + * @brief Partial convolution of Q15 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + /** + * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_fast_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen). + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_fast_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + uint32_t firstIndex, + uint32_t numPoints, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Partial convolution of Q31 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_fast_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + /** + * @brief Partial convolution of Q7 sequences + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_opt_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + uint32_t firstIndex, + uint32_t numPoints, + q15_t * pScratch1, + q15_t * pScratch2); + + +/** + * @brief Partial convolution of Q7 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data + * @param[in] firstIndex is the first output sample to start with. + * @param[in] numPoints is the number of output points to be computed. + * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. + */ + + arm_status arm_conv_partial_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + uint32_t firstIndex, + uint32_t numPoints); + + + + /** + * @brief Instance structure for the Q15 FIR decimator. + */ + + typedef struct + { + uint8_t M; /**< decimation factor. */ + uint16_t numTaps; /**< number of coefficients in the filter. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + } arm_fir_decimate_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR decimator. + */ + + typedef struct + { + uint8_t M; /**< decimation factor. */ + uint16_t numTaps; /**< number of coefficients in the filter. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + + } arm_fir_decimate_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR decimator. + */ + + typedef struct + { + uint8_t M; /**< decimation factor. */ + uint16_t numTaps; /**< number of coefficients in the filter. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + + } arm_fir_decimate_instance_f32; + + + + /** + * @brief Processing function for the floating-point FIR decimator. + * @param[in] *S points to an instance of the floating-point FIR decimator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + * @return none + */ + + void arm_fir_decimate_f32( + const arm_fir_decimate_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point FIR decimator. + * @param[in,out] *S points to an instance of the floating-point FIR decimator structure. + * @param[in] numTaps number of coefficients in the filter. + * @param[in] M decimation factor. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * blockSize is not a multiple of M. + */ + + arm_status arm_fir_decimate_init_f32( + arm_fir_decimate_instance_f32 * S, + uint16_t numTaps, + uint8_t M, + float32_t * pCoeffs, + float32_t * pState, + uint32_t blockSize); + + /** + * @brief Processing function for the Q15 FIR decimator. + * @param[in] *S points to an instance of the Q15 FIR decimator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + * @return none + */ + + void arm_fir_decimate_q15( + const arm_fir_decimate_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4. + * @param[in] *S points to an instance of the Q15 FIR decimator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + * @return none + */ + + void arm_fir_decimate_fast_q15( + const arm_fir_decimate_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + + /** + * @brief Initialization function for the Q15 FIR decimator. + * @param[in,out] *S points to an instance of the Q15 FIR decimator structure. + * @param[in] numTaps number of coefficients in the filter. + * @param[in] M decimation factor. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * blockSize is not a multiple of M. + */ + + arm_status arm_fir_decimate_init_q15( + arm_fir_decimate_instance_q15 * S, + uint16_t numTaps, + uint8_t M, + q15_t * pCoeffs, + q15_t * pState, + uint32_t blockSize); + + /** + * @brief Processing function for the Q31 FIR decimator. + * @param[in] *S points to an instance of the Q31 FIR decimator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + * @return none + */ + + void arm_fir_decimate_q31( + const arm_fir_decimate_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4. + * @param[in] *S points to an instance of the Q31 FIR decimator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of input samples to process per call. + * @return none + */ + + void arm_fir_decimate_fast_q31( + arm_fir_decimate_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 FIR decimator. + * @param[in,out] *S points to an instance of the Q31 FIR decimator structure. + * @param[in] numTaps number of coefficients in the filter. + * @param[in] M decimation factor. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * blockSize is not a multiple of M. + */ + + arm_status arm_fir_decimate_init_q31( + arm_fir_decimate_instance_q31 * S, + uint16_t numTaps, + uint8_t M, + q31_t * pCoeffs, + q31_t * pState, + uint32_t blockSize); + + + + /** + * @brief Instance structure for the Q15 FIR interpolator. + */ + + typedef struct + { + uint8_t L; /**< upsample factor. */ + uint16_t phaseLength; /**< length of each polyphase filter component. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ + q15_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */ + } arm_fir_interpolate_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR interpolator. + */ + + typedef struct + { + uint8_t L; /**< upsample factor. */ + uint16_t phaseLength; /**< length of each polyphase filter component. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ + q31_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */ + } arm_fir_interpolate_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR interpolator. + */ + + typedef struct + { + uint8_t L; /**< upsample factor. */ + uint16_t phaseLength; /**< length of each polyphase filter component. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ + float32_t *pState; /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */ + } arm_fir_interpolate_instance_f32; + + + /** + * @brief Processing function for the Q15 FIR interpolator. + * @param[in] *S points to an instance of the Q15 FIR interpolator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_interpolate_q15( + const arm_fir_interpolate_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 FIR interpolator. + * @param[in,out] *S points to an instance of the Q15 FIR interpolator structure. + * @param[in] L upsample factor. + * @param[in] numTaps number of filter coefficients in the filter. + * @param[in] *pCoeffs points to the filter coefficient buffer. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * the filter length numTaps is not a multiple of the interpolation factor L. + */ + + arm_status arm_fir_interpolate_init_q15( + arm_fir_interpolate_instance_q15 * S, + uint8_t L, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + uint32_t blockSize); + + /** + * @brief Processing function for the Q31 FIR interpolator. + * @param[in] *S points to an instance of the Q15 FIR interpolator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_interpolate_q31( + const arm_fir_interpolate_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q31 FIR interpolator. + * @param[in,out] *S points to an instance of the Q31 FIR interpolator structure. + * @param[in] L upsample factor. + * @param[in] numTaps number of filter coefficients in the filter. + * @param[in] *pCoeffs points to the filter coefficient buffer. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * the filter length numTaps is not a multiple of the interpolation factor L. + */ + + arm_status arm_fir_interpolate_init_q31( + arm_fir_interpolate_instance_q31 * S, + uint8_t L, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the floating-point FIR interpolator. + * @param[in] *S points to an instance of the floating-point FIR interpolator structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_interpolate_f32( + const arm_fir_interpolate_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the floating-point FIR interpolator. + * @param[in,out] *S points to an instance of the floating-point FIR interpolator structure. + * @param[in] L upsample factor. + * @param[in] numTaps number of filter coefficients in the filter. + * @param[in] *pCoeffs points to the filter coefficient buffer. + * @param[in] *pState points to the state buffer. + * @param[in] blockSize number of input samples to process per call. + * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if + * the filter length numTaps is not a multiple of the interpolation factor L. + */ + + arm_status arm_fir_interpolate_init_f32( + arm_fir_interpolate_instance_f32 * S, + uint8_t L, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + uint32_t blockSize); + + /** + * @brief Instance structure for the high precision Q31 Biquad cascade filter. + */ + + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + q63_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */ + q31_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + uint8_t postShift; /**< additional shift, in bits, applied to each output sample. */ + + } arm_biquad_cas_df1_32x64_ins_q31; + + + /** + * @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cas_df1_32x64_q31( + const arm_biquad_cas_df1_32x64_ins_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @param[in,out] *S points to an instance of the high precision Q31 Biquad cascade filter structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] postShift shift to be applied to the output. Varies according to the coefficients format + * @return none + */ + + void arm_biquad_cas_df1_32x64_init_q31( + arm_biquad_cas_df1_32x64_ins_q31 * S, + uint8_t numStages, + q31_t * pCoeffs, + q63_t * pState, + uint8_t postShift); + + + + /** + * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. + */ + + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float32_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */ + float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_cascade_df2T_instance_f32; + + + /** + * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in] *S points to an instance of the filter data structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_biquad_cascade_df2T_f32( + const arm_biquad_cascade_df2T_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter. + * @param[in,out] *S points to an instance of the filter data structure. + * @param[in] numStages number of 2nd order stages in the filter. + * @param[in] *pCoeffs points to the filter coefficients. + * @param[in] *pState points to the state buffer. + * @return none + */ + + void arm_biquad_cascade_df2T_init_f32( + arm_biquad_cascade_df2T_instance_f32 * S, + uint8_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + + + /** + * @brief Instance structure for the Q15 FIR lattice filter. + */ + + typedef struct + { + uint16_t numStages; /**< number of filter stages. */ + q15_t *pState; /**< points to the state variable array. The array is of length numStages. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ + } arm_fir_lattice_instance_q15; + + /** + * @brief Instance structure for the Q31 FIR lattice filter. + */ + + typedef struct + { + uint16_t numStages; /**< number of filter stages. */ + q31_t *pState; /**< points to the state variable array. The array is of length numStages. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ + } arm_fir_lattice_instance_q31; + + /** + * @brief Instance structure for the floating-point FIR lattice filter. + */ + + typedef struct + { + uint16_t numStages; /**< number of filter stages. */ + float32_t *pState; /**< points to the state variable array. The array is of length numStages. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ + } arm_fir_lattice_instance_f32; + + /** + * @brief Initialization function for the Q15 FIR lattice filter. + * @param[in] *S points to an instance of the Q15 FIR lattice structure. + * @param[in] numStages number of filter stages. + * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages. + * @param[in] *pState points to the state buffer. The array is of length numStages. + * @return none. + */ + + void arm_fir_lattice_init_q15( + arm_fir_lattice_instance_q15 * S, + uint16_t numStages, + q15_t * pCoeffs, + q15_t * pState); + + + /** + * @brief Processing function for the Q15 FIR lattice filter. + * @param[in] *S points to an instance of the Q15 FIR lattice structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + void arm_fir_lattice_q15( + const arm_fir_lattice_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q31 FIR lattice filter. + * @param[in] *S points to an instance of the Q31 FIR lattice structure. + * @param[in] numStages number of filter stages. + * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages. + * @param[in] *pState points to the state buffer. The array is of length numStages. + * @return none. + */ + + void arm_fir_lattice_init_q31( + arm_fir_lattice_instance_q31 * S, + uint16_t numStages, + q31_t * pCoeffs, + q31_t * pState); + + + /** + * @brief Processing function for the Q31 FIR lattice filter. + * @param[in] *S points to an instance of the Q31 FIR lattice structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_fir_lattice_q31( + const arm_fir_lattice_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + +/** + * @brief Initialization function for the floating-point FIR lattice filter. + * @param[in] *S points to an instance of the floating-point FIR lattice structure. + * @param[in] numStages number of filter stages. + * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages. + * @param[in] *pState points to the state buffer. The array is of length numStages. + * @return none. + */ + + void arm_fir_lattice_init_f32( + arm_fir_lattice_instance_f32 * S, + uint16_t numStages, + float32_t * pCoeffs, + float32_t * pState); + + /** + * @brief Processing function for the floating-point FIR lattice filter. + * @param[in] *S points to an instance of the floating-point FIR lattice structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_fir_lattice_f32( + const arm_fir_lattice_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Instance structure for the Q15 IIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of stages in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ + q15_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ + q15_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ + } arm_iir_lattice_instance_q15; + + /** + * @brief Instance structure for the Q31 IIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of stages in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ + q31_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ + q31_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ + } arm_iir_lattice_instance_q31; + + /** + * @brief Instance structure for the floating-point IIR lattice filter. + */ + typedef struct + { + uint16_t numStages; /**< number of stages in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ + float32_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ + float32_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ + } arm_iir_lattice_instance_f32; + + /** + * @brief Processing function for the floating-point IIR lattice filter. + * @param[in] *S points to an instance of the floating-point IIR lattice structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_iir_lattice_f32( + const arm_iir_lattice_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @brief Initialization function for the floating-point IIR lattice filter. + * @param[in] *S points to an instance of the floating-point IIR lattice structure. + * @param[in] numStages number of stages in the filter. + * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages. + * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1. + * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize-1. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_iir_lattice_init_f32( + arm_iir_lattice_instance_f32 * S, + uint16_t numStages, + float32_t * pkCoeffs, + float32_t * pvCoeffs, + float32_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q31 IIR lattice filter. + * @param[in] *S points to an instance of the Q31 IIR lattice structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_iir_lattice_q31( + const arm_iir_lattice_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q31 IIR lattice filter. + * @param[in] *S points to an instance of the Q31 IIR lattice structure. + * @param[in] numStages number of stages in the filter. + * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages. + * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1. + * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_iir_lattice_init_q31( + arm_iir_lattice_instance_q31 * S, + uint16_t numStages, + q31_t * pkCoeffs, + q31_t * pvCoeffs, + q31_t * pState, + uint32_t blockSize); + + + /** + * @brief Processing function for the Q15 IIR lattice filter. + * @param[in] *S points to an instance of the Q15 IIR lattice structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_iir_lattice_q15( + const arm_iir_lattice_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + +/** + * @brief Initialization function for the Q15 IIR lattice filter. + * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure. + * @param[in] numStages number of stages in the filter. + * @param[in] *pkCoeffs points to reflection coefficient buffer. The array is of length numStages. + * @param[in] *pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1. + * @param[in] *pState points to state buffer. The array is of length numStages+blockSize. + * @param[in] blockSize number of samples to process per call. + * @return none. + */ + + void arm_iir_lattice_init_q15( + arm_iir_lattice_instance_q15 * S, + uint16_t numStages, + q15_t * pkCoeffs, + q15_t * pvCoeffs, + q15_t * pState, + uint32_t blockSize); + + /** + * @brief Instance structure for the floating-point LMS filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + float32_t mu; /**< step size that controls filter coefficient updates. */ + } arm_lms_instance_f32; + + /** + * @brief Processing function for floating-point LMS filter. + * @param[in] *S points to an instance of the floating-point LMS filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[in] *pRef points to the block of reference data. + * @param[out] *pOut points to the block of output data. + * @param[out] *pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_f32( + const arm_lms_instance_f32 * S, + float32_t * pSrc, + float32_t * pRef, + float32_t * pOut, + float32_t * pErr, + uint32_t blockSize); + + /** + * @brief Initialization function for floating-point LMS filter. + * @param[in] *S points to an instance of the floating-point LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] *pCoeffs points to the coefficient buffer. + * @param[in] *pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_init_f32( + arm_lms_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + float32_t mu, + uint32_t blockSize); + + /** + * @brief Instance structure for the Q15 LMS filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q15_t mu; /**< step size that controls filter coefficient updates. */ + uint32_t postShift; /**< bit shift applied to coefficients. */ + } arm_lms_instance_q15; + + + /** + * @brief Initialization function for the Q15 LMS filter. + * @param[in] *S points to an instance of the Q15 LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] *pCoeffs points to the coefficient buffer. + * @param[in] *pState points to the state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + * @return none. + */ + + void arm_lms_init_q15( + arm_lms_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + q15_t mu, + uint32_t blockSize, + uint32_t postShift); + + /** + * @brief Processing function for Q15 LMS filter. + * @param[in] *S points to an instance of the Q15 LMS filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[in] *pRef points to the block of reference data. + * @param[out] *pOut points to the block of output data. + * @param[out] *pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_q15( + const arm_lms_instance_q15 * S, + q15_t * pSrc, + q15_t * pRef, + q15_t * pOut, + q15_t * pErr, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q31 LMS filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q31_t mu; /**< step size that controls filter coefficient updates. */ + uint32_t postShift; /**< bit shift applied to coefficients. */ + + } arm_lms_instance_q31; + + /** + * @brief Processing function for Q31 LMS filter. + * @param[in] *S points to an instance of the Q15 LMS filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[in] *pRef points to the block of reference data. + * @param[out] *pOut points to the block of output data. + * @param[out] *pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_q31( + const arm_lms_instance_q31 * S, + q31_t * pSrc, + q31_t * pRef, + q31_t * pOut, + q31_t * pErr, + uint32_t blockSize); + + /** + * @brief Initialization function for Q31 LMS filter. + * @param[in] *S points to an instance of the Q31 LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] *pCoeffs points to coefficient buffer. + * @param[in] *pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + * @return none. + */ + + void arm_lms_init_q31( + arm_lms_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + q31_t mu, + uint32_t blockSize, + uint32_t postShift); + + /** + * @brief Instance structure for the floating-point normalized LMS filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + float32_t mu; /**< step size that control filter coefficient updates. */ + float32_t energy; /**< saves previous frame energy. */ + float32_t x0; /**< saves previous input sample. */ + } arm_lms_norm_instance_f32; + + /** + * @brief Processing function for floating-point normalized LMS filter. + * @param[in] *S points to an instance of the floating-point normalized LMS filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[in] *pRef points to the block of reference data. + * @param[out] *pOut points to the block of output data. + * @param[out] *pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_norm_f32( + arm_lms_norm_instance_f32 * S, + float32_t * pSrc, + float32_t * pRef, + float32_t * pOut, + float32_t * pErr, + uint32_t blockSize); + + /** + * @brief Initialization function for floating-point normalized LMS filter. + * @param[in] *S points to an instance of the floating-point LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] *pCoeffs points to coefficient buffer. + * @param[in] *pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_norm_init_f32( + arm_lms_norm_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + float32_t mu, + uint32_t blockSize); + + + /** + * @brief Instance structure for the Q31 normalized LMS filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q31_t mu; /**< step size that controls filter coefficient updates. */ + uint8_t postShift; /**< bit shift applied to coefficients. */ + q31_t *recipTable; /**< points to the reciprocal initial value table. */ + q31_t energy; /**< saves previous frame energy. */ + q31_t x0; /**< saves previous input sample. */ + } arm_lms_norm_instance_q31; + + /** + * @brief Processing function for Q31 normalized LMS filter. + * @param[in] *S points to an instance of the Q31 normalized LMS filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[in] *pRef points to the block of reference data. + * @param[out] *pOut points to the block of output data. + * @param[out] *pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_norm_q31( + arm_lms_norm_instance_q31 * S, + q31_t * pSrc, + q31_t * pRef, + q31_t * pOut, + q31_t * pErr, + uint32_t blockSize); + + /** + * @brief Initialization function for Q31 normalized LMS filter. + * @param[in] *S points to an instance of the Q31 normalized LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] *pCoeffs points to coefficient buffer. + * @param[in] *pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + * @return none. + */ + + void arm_lms_norm_init_q31( + arm_lms_norm_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + q31_t mu, + uint32_t blockSize, + uint8_t postShift); + + /** + * @brief Instance structure for the Q15 normalized LMS filter. + */ + + typedef struct + { + uint16_t numTaps; /**< Number of coefficients in the filter. */ + q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ + q15_t mu; /**< step size that controls filter coefficient updates. */ + uint8_t postShift; /**< bit shift applied to coefficients. */ + q15_t *recipTable; /**< Points to the reciprocal initial value table. */ + q15_t energy; /**< saves previous frame energy. */ + q15_t x0; /**< saves previous input sample. */ + } arm_lms_norm_instance_q15; + + /** + * @brief Processing function for Q15 normalized LMS filter. + * @param[in] *S points to an instance of the Q15 normalized LMS filter structure. + * @param[in] *pSrc points to the block of input data. + * @param[in] *pRef points to the block of reference data. + * @param[out] *pOut points to the block of output data. + * @param[out] *pErr points to the block of error data. + * @param[in] blockSize number of samples to process. + * @return none. + */ + + void arm_lms_norm_q15( + arm_lms_norm_instance_q15 * S, + q15_t * pSrc, + q15_t * pRef, + q15_t * pOut, + q15_t * pErr, + uint32_t blockSize); + + + /** + * @brief Initialization function for Q15 normalized LMS filter. + * @param[in] *S points to an instance of the Q15 normalized LMS filter structure. + * @param[in] numTaps number of filter coefficients. + * @param[in] *pCoeffs points to coefficient buffer. + * @param[in] *pState points to state buffer. + * @param[in] mu step size that controls filter coefficient updates. + * @param[in] blockSize number of samples to process. + * @param[in] postShift bit shift applied to coefficients. + * @return none. + */ + + void arm_lms_norm_init_q15( + arm_lms_norm_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + q15_t mu, + uint32_t blockSize, + uint8_t postShift); + + /** + * @brief Correlation of floating-point sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @return none. + */ + + void arm_correlate_f32( + float32_t * pSrcA, + uint32_t srcALen, + float32_t * pSrcB, + uint32_t srcBLen, + float32_t * pDst); + + + /** + * @brief Correlation of Q15 sequences + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @return none. + */ + void arm_correlate_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch); + + + /** + * @brief Correlation of Q15 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @return none. + */ + + void arm_correlate_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + /** + * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @return none. + */ + + void arm_correlate_fast_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst); + + + + /** + * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @return none. + */ + + void arm_correlate_fast_opt_q15( + q15_t * pSrcA, + uint32_t srcALen, + q15_t * pSrcB, + uint32_t srcBLen, + q15_t * pDst, + q15_t * pScratch); + + /** + * @brief Correlation of Q31 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @return none. + */ + + void arm_correlate_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + /** + * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @return none. + */ + + void arm_correlate_fast_q31( + q31_t * pSrcA, + uint32_t srcALen, + q31_t * pSrcB, + uint32_t srcBLen, + q31_t * pDst); + + + + /** + * @brief Correlation of Q7 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. + * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). + * @return none. + */ + + void arm_correlate_opt_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst, + q15_t * pScratch1, + q15_t * pScratch2); + + + /** + * @brief Correlation of Q7 sequences. + * @param[in] *pSrcA points to the first input sequence. + * @param[in] srcALen length of the first input sequence. + * @param[in] *pSrcB points to the second input sequence. + * @param[in] srcBLen length of the second input sequence. + * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. + * @return none. + */ + + void arm_correlate_q7( + q7_t * pSrcA, + uint32_t srcALen, + q7_t * pSrcB, + uint32_t srcBLen, + q7_t * pDst); + + + /** + * @brief Instance structure for the floating-point sparse FIR filter. + */ + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + float32_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_f32; + + /** + * @brief Instance structure for the Q31 sparse FIR filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + q31_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_q31; + + /** + * @brief Instance structure for the Q15 sparse FIR filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + q15_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_q15; + + /** + * @brief Instance structure for the Q7 sparse FIR filter. + */ + + typedef struct + { + uint16_t numTaps; /**< number of coefficients in the filter. */ + uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ + q7_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ + q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ + uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ + int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ + } arm_fir_sparse_instance_q7; + + /** + * @brief Processing function for the floating-point sparse FIR filter. + * @param[in] *S points to an instance of the floating-point sparse FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] *pScratchIn points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_sparse_f32( + arm_fir_sparse_instance_f32 * S, + float32_t * pSrc, + float32_t * pDst, + float32_t * pScratchIn, + uint32_t blockSize); + + /** + * @brief Initialization function for the floating-point sparse FIR filter. + * @param[in,out] *S points to an instance of the floating-point sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] *pCoeffs points to the array of filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] *pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + * @return none + */ + + void arm_fir_sparse_init_f32( + arm_fir_sparse_instance_f32 * S, + uint16_t numTaps, + float32_t * pCoeffs, + float32_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + /** + * @brief Processing function for the Q31 sparse FIR filter. + * @param[in] *S points to an instance of the Q31 sparse FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] *pScratchIn points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_sparse_q31( + arm_fir_sparse_instance_q31 * S, + q31_t * pSrc, + q31_t * pDst, + q31_t * pScratchIn, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q31 sparse FIR filter. + * @param[in,out] *S points to an instance of the Q31 sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] *pCoeffs points to the array of filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] *pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + * @return none + */ + + void arm_fir_sparse_init_q31( + arm_fir_sparse_instance_q31 * S, + uint16_t numTaps, + q31_t * pCoeffs, + q31_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + /** + * @brief Processing function for the Q15 sparse FIR filter. + * @param[in] *S points to an instance of the Q15 sparse FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] *pScratchIn points to a temporary buffer of size blockSize. + * @param[in] *pScratchOut points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_sparse_q15( + arm_fir_sparse_instance_q15 * S, + q15_t * pSrc, + q15_t * pDst, + q15_t * pScratchIn, + q31_t * pScratchOut, + uint32_t blockSize); + + + /** + * @brief Initialization function for the Q15 sparse FIR filter. + * @param[in,out] *S points to an instance of the Q15 sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] *pCoeffs points to the array of filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] *pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + * @return none + */ + + void arm_fir_sparse_init_q15( + arm_fir_sparse_instance_q15 * S, + uint16_t numTaps, + q15_t * pCoeffs, + q15_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + /** + * @brief Processing function for the Q7 sparse FIR filter. + * @param[in] *S points to an instance of the Q7 sparse FIR structure. + * @param[in] *pSrc points to the block of input data. + * @param[out] *pDst points to the block of output data + * @param[in] *pScratchIn points to a temporary buffer of size blockSize. + * @param[in] *pScratchOut points to a temporary buffer of size blockSize. + * @param[in] blockSize number of input samples to process per call. + * @return none. + */ + + void arm_fir_sparse_q7( + arm_fir_sparse_instance_q7 * S, + q7_t * pSrc, + q7_t * pDst, + q7_t * pScratchIn, + q31_t * pScratchOut, + uint32_t blockSize); + + /** + * @brief Initialization function for the Q7 sparse FIR filter. + * @param[in,out] *S points to an instance of the Q7 sparse FIR structure. + * @param[in] numTaps number of nonzero coefficients in the filter. + * @param[in] *pCoeffs points to the array of filter coefficients. + * @param[in] *pState points to the state buffer. + * @param[in] *pTapDelay points to the array of offset times. + * @param[in] maxDelay maximum offset time supported. + * @param[in] blockSize number of samples that will be processed per block. + * @return none + */ + + void arm_fir_sparse_init_q7( + arm_fir_sparse_instance_q7 * S, + uint16_t numTaps, + q7_t * pCoeffs, + q7_t * pState, + int32_t * pTapDelay, + uint16_t maxDelay, + uint32_t blockSize); + + + /* + * @brief Floating-point sin_cos function. + * @param[in] theta input value in degrees + * @param[out] *pSinVal points to the processed sine output. + * @param[out] *pCosVal points to the processed cos output. + * @return none. + */ + + void arm_sin_cos_f32( + float32_t theta, + float32_t * pSinVal, + float32_t * pCcosVal); + + /* + * @brief Q31 sin_cos function. + * @param[in] theta scaled input value in degrees + * @param[out] *pSinVal points to the processed sine output. + * @param[out] *pCosVal points to the processed cosine output. + * @return none. + */ + + void arm_sin_cos_q31( + q31_t theta, + q31_t * pSinVal, + q31_t * pCosVal); + + + /** + * @brief Floating-point complex conjugate. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + * @return none. + */ + + void arm_cmplx_conj_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples); + + /** + * @brief Q31 complex conjugate. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + * @return none. + */ + + void arm_cmplx_conj_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples); + + /** + * @brief Q15 complex conjugate. + * @param[in] *pSrc points to the input vector + * @param[out] *pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + * @return none. + */ + + void arm_cmplx_conj_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples); + + + + /** + * @brief Floating-point complex magnitude squared + * @param[in] *pSrc points to the complex input vector + * @param[out] *pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + * @return none. + */ + + void arm_cmplx_mag_squared_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples); + + /** + * @brief Q31 complex magnitude squared + * @param[in] *pSrc points to the complex input vector + * @param[out] *pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + * @return none. + */ + + void arm_cmplx_mag_squared_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples); + + /** + * @brief Q15 complex magnitude squared + * @param[in] *pSrc points to the complex input vector + * @param[out] *pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + * @return none. + */ + + void arm_cmplx_mag_squared_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples); + + + /** + * @ingroup groupController + */ + + /** + * @defgroup PID PID Motor Control + * + * A Proportional Integral Derivative (PID) controller is a generic feedback control + * loop mechanism widely used in industrial control systems. + * A PID controller is the most commonly used type of feedback controller. + * + * This set of functions implements (PID) controllers + * for Q15, Q31, and floating-point data types. The functions operate on a single sample + * of data and each call to the function returns a single processed value. + * S points to an instance of the PID control data structure. in + * is the input sample value. The functions return the output value. + * + * \par Algorithm: + * 
+   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
+   *    A0 = Kp + Ki + Kd
+   *    A1 = (-Kp ) - (2 * Kd )
+   *    A2 = Kd
+ * + * \par + * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant + * + * \par + * \image html PID.gif "Proportional Integral Derivative Controller" + * + * \par + * The PID controller calculates an "error" value as the difference between + * the measured output and the reference input. + * The controller attempts to minimize the error by adjusting the process control inputs. + * The proportional value determines the reaction to the current error, + * the integral value determines the reaction based on the sum of recent errors, + * and the derivative value determines the reaction based on the rate at which the error has been changing. + * + * \par Instance Structure + * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure. + * A separate instance structure must be defined for each PID Controller. + * There are separate instance structure declarations for each of the 3 supported data types. + * + * \par Reset Functions + * There is also an associated reset function for each data type which clears the state array. + * + * \par Initialization Functions + * There is also an associated initialization function for each data type. + * The initialization function performs the following operations: + * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains. + * - Zeros out the values in the state buffer. + * + * \par + * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function. + * + * \par Fixed-Point Behavior + * Care must be taken when using the fixed-point versions of the PID Controller functions. + * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup PID + * @{ + */ + + /** + * @brief Process function for the floating-point PID Control. + * @param[in,out] *S is an instance of the floating-point PID Control structure + * @param[in] in input sample to process + * @return out processed output sample. + */ + + + static __INLINE float32_t arm_pid_f32( + arm_pid_instance_f32 * S, + float32_t in) + { + float32_t out; + + /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */ + out = (S->A0 * in) + + (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]); + + /* Update state */ + S->state[1] = S->state[0]; + S->state[0] = in; + S->state[2] = out; + + /* return to application */ + return (out); + + } + + /** + * @brief Process function for the Q31 PID Control. + * @param[in,out] *S points to an instance of the Q31 PID Control structure + * @param[in] in input sample to process + * @return out processed output sample. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 64-bit accumulator. + * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit. + * Thus, if the accumulator result overflows it wraps around rather than clip. + * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions. + * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format. + */ + + static __INLINE q31_t arm_pid_q31( + arm_pid_instance_q31 * S, + q31_t in) + { + q63_t acc; + q31_t out; + + /* acc = A0 * x[n] */ + acc = (q63_t) S->A0 * in; + + /* acc += A1 * x[n-1] */ + acc += (q63_t) S->A1 * S->state[0]; + + /* acc += A2 * x[n-2] */ + acc += (q63_t) S->A2 * S->state[1]; + + /* convert output to 1.31 format to add y[n-1] */ + out = (q31_t) (acc >> 31u); + + /* out += y[n-1] */ + out += S->state[2]; + + /* Update state */ + S->state[1] = S->state[0]; + S->state[0] = in; + S->state[2] = out; + + /* return to application */ + return (out); + + } + + /** + * @brief Process function for the Q15 PID Control. + * @param[in,out] *S points to an instance of the Q15 PID Control structure + * @param[in] in input sample to process + * @return out processed output sample. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using a 64-bit internal accumulator. + * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result. + * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format. + * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved. + * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits. + * Lastly, the accumulator is saturated to yield a result in 1.15 format. + */ + + static __INLINE q15_t arm_pid_q15( + arm_pid_instance_q15 * S, + q15_t in) + { + q63_t acc; + q15_t out; + +#ifndef ARM_MATH_CM0_FAMILY + __SIMD32_TYPE *vstate; + + /* Implementation of PID controller */ + + /* acc = A0 * x[n] */ + acc = (q31_t) __SMUAD(S->A0, in); + + /* acc += A1 * x[n-1] + A2 * x[n-2] */ + vstate = __SIMD32_CONST(S->state); + acc = __SMLALD(S->A1, (q31_t) *vstate, acc); + +#else + /* acc = A0 * x[n] */ + acc = ((q31_t) S->A0) * in; + + /* acc += A1 * x[n-1] + A2 * x[n-2] */ + acc += (q31_t) S->A1 * S->state[0]; + acc += (q31_t) S->A2 * S->state[1]; + +#endif + + /* acc += y[n-1] */ + acc += (q31_t) S->state[2] << 15; + + /* saturate the output */ + out = (q15_t) (__SSAT((acc >> 15), 16)); + + /* Update state */ + S->state[1] = S->state[0]; + S->state[0] = in; + S->state[2] = out; + + /* return to application */ + return (out); + + } + + /** + * @} end of PID group + */ + + + /** + * @brief Floating-point matrix inverse. + * @param[in] *src points to the instance of the input floating-point matrix structure. + * @param[out] *dst points to the instance of the output floating-point matrix structure. + * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match. + * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR. + */ + + arm_status arm_mat_inverse_f32( + const arm_matrix_instance_f32 * src, + arm_matrix_instance_f32 * dst); + + + + /** + * @ingroup groupController + */ + + + /** + * @defgroup clarke Vector Clarke Transform + * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector. + * Generally the Clarke transform uses three-phase currents Ia, Ib and Ic to calculate currents + * in the two-phase orthogonal stator axis Ialpha and Ibeta. + * When Ialpha is superposed with Ia as shown in the figure below + * \image html clarke.gif Stator current space vector and its components in (a,b). + * and Ia + Ib + Ic = 0, in this condition Ialpha and Ibeta + * can be calculated using only Ia and Ib. + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html clarkeFormula.gif + * where Ia and Ib are the instantaneous stator phases and + * pIalpha and pIbeta are the two coordinates of time invariant vector. + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Clarke transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup clarke + * @{ + */ + + /** + * + * @brief Floating-point Clarke transform + * @param[in] Ia input three-phase coordinate a + * @param[in] Ib input three-phase coordinate b + * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta + * @return none. + */ + + static __INLINE void arm_clarke_f32( + float32_t Ia, + float32_t Ib, + float32_t * pIalpha, + float32_t * pIbeta) + { + /* Calculate pIalpha using the equation, pIalpha = Ia */ + *pIalpha = Ia; + + /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */ + *pIbeta = + ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib); + + } + + /** + * @brief Clarke transform for Q31 version + * @param[in] Ia input three-phase coordinate a + * @param[in] Ib input three-phase coordinate b + * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta + * @return none. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the addition, hence there is no risk of overflow. + */ + + static __INLINE void arm_clarke_q31( + q31_t Ia, + q31_t Ib, + q31_t * pIalpha, + q31_t * pIbeta) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + + /* Calculating pIalpha from Ia by equation pIalpha = Ia */ + *pIalpha = Ia; + + /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */ + product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30); + + /* Intermediate product is calculated by (2/sqrt(3) * Ib) */ + product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30); + + /* pIbeta is calculated by adding the intermediate products */ + *pIbeta = __QADD(product1, product2); + } + + /** + * @} end of clarke group + */ + + /** + * @brief Converts the elements of the Q7 vector to Q31 vector. + * @param[in] *pSrc input pointer + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_q7_to_q31( + q7_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + + + /** + * @ingroup groupController + */ + + /** + * @defgroup inv_clarke Vector Inverse Clarke Transform + * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases. + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html clarkeInvFormula.gif + * where pIa and pIb are the instantaneous stator phases and + * Ialpha and Ibeta are the two coordinates of time invariant vector. + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Clarke transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup inv_clarke + * @{ + */ + + /** + * @brief Floating-point Inverse Clarke transform + * @param[in] Ialpha input two-phase orthogonal vector axis alpha + * @param[in] Ibeta input two-phase orthogonal vector axis beta + * @param[out] *pIa points to output three-phase coordinate a + * @param[out] *pIb points to output three-phase coordinate b + * @return none. + */ + + + static __INLINE void arm_inv_clarke_f32( + float32_t Ialpha, + float32_t Ibeta, + float32_t * pIa, + float32_t * pIb) + { + /* Calculating pIa from Ialpha by equation pIa = Ialpha */ + *pIa = Ialpha; + + /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */ + *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta; + + } + + /** + * @brief Inverse Clarke transform for Q31 version + * @param[in] Ialpha input two-phase orthogonal vector axis alpha + * @param[in] Ibeta input two-phase orthogonal vector axis beta + * @param[out] *pIa points to output three-phase coordinate a + * @param[out] *pIb points to output three-phase coordinate b + * @return none. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the subtraction, hence there is no risk of overflow. + */ + + static __INLINE void arm_inv_clarke_q31( + q31_t Ialpha, + q31_t Ibeta, + q31_t * pIa, + q31_t * pIb) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + + /* Calculating pIa from Ialpha by equation pIa = Ialpha */ + *pIa = Ialpha; + + /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */ + product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31); + + /* Intermediate product is calculated by (1/sqrt(3) * pIb) */ + product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31); + + /* pIb is calculated by subtracting the products */ + *pIb = __QSUB(product2, product1); + + } + + /** + * @} end of inv_clarke group + */ + + /** + * @brief Converts the elements of the Q7 vector to Q15 vector. + * @param[in] *pSrc input pointer + * @param[out] *pDst output pointer + * @param[in] blockSize number of samples to process + * @return none. + */ + void arm_q7_to_q15( + q7_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + + + /** + * @ingroup groupController + */ + + /** + * @defgroup park Vector Park Transform + * + * Forward Park transform converts the input two-coordinate vector to flux and torque components. + * The Park transform can be used to realize the transformation of the Ialpha and the Ibeta currents + * from the stationary to the moving reference frame and control the spatial relationship between + * the stator vector current and rotor flux vector. + * If we consider the d axis aligned with the rotor flux, the diagram below shows the + * current vector and the relationship from the two reference frames: + * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame" + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html parkFormula.gif + * where Ialpha and Ibeta are the stator vector components, + * pId and pIq are rotor vector components and cosVal and sinVal are the + * cosine and sine values of theta (rotor flux position). + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Park transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup park + * @{ + */ + + /** + * @brief Floating-point Park transform + * @param[in] Ialpha input two-phase vector coordinate alpha + * @param[in] Ibeta input two-phase vector coordinate beta + * @param[out] *pId points to output rotor reference frame d + * @param[out] *pIq points to output rotor reference frame q + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * @return none. + * + * The function implements the forward Park transform. + * + */ + + static __INLINE void arm_park_f32( + float32_t Ialpha, + float32_t Ibeta, + float32_t * pId, + float32_t * pIq, + float32_t sinVal, + float32_t cosVal) + { + /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */ + *pId = Ialpha * cosVal + Ibeta * sinVal; + + /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */ + *pIq = -Ialpha * sinVal + Ibeta * cosVal; + + } + + /** + * @brief Park transform for Q31 version + * @param[in] Ialpha input two-phase vector coordinate alpha + * @param[in] Ibeta input two-phase vector coordinate beta + * @param[out] *pId points to output rotor reference frame d + * @param[out] *pIq points to output rotor reference frame q + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * @return none. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the addition and subtraction, hence there is no risk of overflow. + */ + + + static __INLINE void arm_park_q31( + q31_t Ialpha, + q31_t Ibeta, + q31_t * pId, + q31_t * pIq, + q31_t sinVal, + q31_t cosVal) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + q31_t product3, product4; /* Temporary variables used to store intermediate results */ + + /* Intermediate product is calculated by (Ialpha * cosVal) */ + product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31); + + /* Intermediate product is calculated by (Ibeta * sinVal) */ + product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31); + + + /* Intermediate product is calculated by (Ialpha * sinVal) */ + product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31); + + /* Intermediate product is calculated by (Ibeta * cosVal) */ + product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31); + + /* Calculate pId by adding the two intermediate products 1 and 2 */ + *pId = __QADD(product1, product2); + + /* Calculate pIq by subtracting the two intermediate products 3 from 4 */ + *pIq = __QSUB(product4, product3); + } + + /** + * @} end of park group + */ + + /** + * @brief Converts the elements of the Q7 vector to floating-point vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q7_to_float( + q7_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @ingroup groupController + */ + + /** + * @defgroup inv_park Vector Inverse Park transform + * Inverse Park transform converts the input flux and torque components to two-coordinate vector. + * + * The function operates on a single sample of data and each call to the function returns the processed output. + * The library provides separate functions for Q31 and floating-point data types. + * \par Algorithm + * \image html parkInvFormula.gif + * where pIalpha and pIbeta are the stator vector components, + * Id and Iq are rotor vector components and cosVal and sinVal are the + * cosine and sine values of theta (rotor flux position). + * \par Fixed-Point Behavior + * Care must be taken when using the Q31 version of the Park transform. + * In particular, the overflow and saturation behavior of the accumulator used must be considered. + * Refer to the function specific documentation below for usage guidelines. + */ + + /** + * @addtogroup inv_park + * @{ + */ + + /** + * @brief Floating-point Inverse Park transform + * @param[in] Id input coordinate of rotor reference frame d + * @param[in] Iq input coordinate of rotor reference frame q + * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * @return none. + */ + + static __INLINE void arm_inv_park_f32( + float32_t Id, + float32_t Iq, + float32_t * pIalpha, + float32_t * pIbeta, + float32_t sinVal, + float32_t cosVal) + { + /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */ + *pIalpha = Id * cosVal - Iq * sinVal; + + /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */ + *pIbeta = Id * sinVal + Iq * cosVal; + + } + + + /** + * @brief Inverse Park transform for Q31 version + * @param[in] Id input coordinate of rotor reference frame d + * @param[in] Iq input coordinate of rotor reference frame q + * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha + * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta + * @param[in] sinVal sine value of rotation angle theta + * @param[in] cosVal cosine value of rotation angle theta + * @return none. + * + * Scaling and Overflow Behavior: + * \par + * The function is implemented using an internal 32-bit accumulator. + * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. + * There is saturation on the addition, hence there is no risk of overflow. + */ + + + static __INLINE void arm_inv_park_q31( + q31_t Id, + q31_t Iq, + q31_t * pIalpha, + q31_t * pIbeta, + q31_t sinVal, + q31_t cosVal) + { + q31_t product1, product2; /* Temporary variables used to store intermediate results */ + q31_t product3, product4; /* Temporary variables used to store intermediate results */ + + /* Intermediate product is calculated by (Id * cosVal) */ + product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31); + + /* Intermediate product is calculated by (Iq * sinVal) */ + product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31); + + + /* Intermediate product is calculated by (Id * sinVal) */ + product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31); + + /* Intermediate product is calculated by (Iq * cosVal) */ + product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31); + + /* Calculate pIalpha by using the two intermediate products 1 and 2 */ + *pIalpha = __QSUB(product1, product2); + + /* Calculate pIbeta by using the two intermediate products 3 and 4 */ + *pIbeta = __QADD(product4, product3); + + } + + /** + * @} end of Inverse park group + */ + + + /** + * @brief Converts the elements of the Q31 vector to floating-point vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q31_to_float( + q31_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + /** + * @ingroup groupInterpolation + */ + + /** + * @defgroup LinearInterpolate Linear Interpolation + * + * Linear interpolation is a method of curve fitting using linear polynomials. + * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line + * + * \par + * \image html LinearInterp.gif "Linear interpolation" + * + * \par + * A Linear Interpolate function calculates an output value(y), for the input(x) + * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values) + * + * \par Algorithm: + * 
+   *       y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
+   *       where x0, x1 are nearest values of input x
+   *             y0, y1 are nearest values to output y
+   *
+ * + * \par + * This set of functions implements Linear interpolation process + * for Q7, Q15, Q31, and floating-point data types. The functions operate on a single + * sample of data and each call to the function returns a single processed value. + * S points to an instance of the Linear Interpolate function data structure. + * x is the input sample value. The functions returns the output value. + * + * \par + * if x is outside of the table boundary, Linear interpolation returns first value of the table + * if x is below input range and returns last value of table if x is above range. + */ + + /** + * @addtogroup LinearInterpolate + * @{ + */ + + /** + * @brief Process function for the floating-point Linear Interpolation Function. + * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure + * @param[in] x input sample to process + * @return y processed output sample. + * + */ + + static __INLINE float32_t arm_linear_interp_f32( + arm_linear_interp_instance_f32 * S, + float32_t x) + { + + float32_t y; + float32_t x0, x1; /* Nearest input values */ + float32_t y0, y1; /* Nearest output values */ + float32_t xSpacing = S->xSpacing; /* spacing between input values */ + int32_t i; /* Index variable */ + float32_t *pYData = S->pYData; /* pointer to output table */ + + /* Calculation of index */ + i = (int32_t) ((x - S->x1) / xSpacing); + + if(i < 0) + { + /* Iniatilize output for below specified range as least output value of table */ + y = pYData[0]; + } + else if((uint32_t)i >= S->nValues) + { + /* Iniatilize output for above specified range as last output value of table */ + y = pYData[S->nValues - 1]; + } + else + { + /* Calculation of nearest input values */ + x0 = S->x1 + i * xSpacing; + x1 = S->x1 + (i + 1) * xSpacing; + + /* Read of nearest output values */ + y0 = pYData[i]; + y1 = pYData[i + 1]; + + /* Calculation of output */ + y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0)); + + } + + /* returns output value */ + return (y); + } + + /** + * + * @brief Process function for the Q31 Linear Interpolation Function. + * @param[in] *pYData pointer to Q31 Linear Interpolation table + * @param[in] x input sample to process + * @param[in] nValues number of table values + * @return y processed output sample. + * + * \par + * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. + * This function can support maximum of table size 2^12. + * + */ + + + static __INLINE q31_t arm_linear_interp_q31( + q31_t * pYData, + q31_t x, + uint32_t nValues) + { + q31_t y; /* output */ + q31_t y0, y1; /* Nearest output values */ + q31_t fract; /* fractional part */ + int32_t index; /* Index to read nearest output values */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + index = ((x & 0xFFF00000) >> 20); + + if(index >= (int32_t)(nValues - 1)) + { + return (pYData[nValues - 1]); + } + else if(index < 0) + { + return (pYData[0]); + } + else + { + + /* 20 bits for the fractional part */ + /* shift left by 11 to keep fract in 1.31 format */ + fract = (x & 0x000FFFFF) << 11; + + /* Read two nearest output values from the index in 1.31(q31) format */ + y0 = pYData[index]; + y1 = pYData[index + 1u]; + + /* Calculation of y0 * (1-fract) and y is in 2.30 format */ + y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32)); + + /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */ + y += ((q31_t) (((q63_t) y1 * fract) >> 32)); + + /* Convert y to 1.31 format */ + return (y << 1u); + + } + + } + + /** + * + * @brief Process function for the Q15 Linear Interpolation Function. + * @param[in] *pYData pointer to Q15 Linear Interpolation table + * @param[in] x input sample to process + * @param[in] nValues number of table values + * @return y processed output sample. + * + * \par + * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. + * This function can support maximum of table size 2^12. + * + */ + + + static __INLINE q15_t arm_linear_interp_q15( + q15_t * pYData, + q31_t x, + uint32_t nValues) + { + q63_t y; /* output */ + q15_t y0, y1; /* Nearest output values */ + q31_t fract; /* fractional part */ + int32_t index; /* Index to read nearest output values */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + index = ((x & 0xFFF00000) >> 20u); + + if(index >= (int32_t)(nValues - 1)) + { + return (pYData[nValues - 1]); + } + else if(index < 0) + { + return (pYData[0]); + } + else + { + /* 20 bits for the fractional part */ + /* fract is in 12.20 format */ + fract = (x & 0x000FFFFF); + + /* Read two nearest output values from the index */ + y0 = pYData[index]; + y1 = pYData[index + 1u]; + + /* Calculation of y0 * (1-fract) and y is in 13.35 format */ + y = ((q63_t) y0 * (0xFFFFF - fract)); + + /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */ + y += ((q63_t) y1 * (fract)); + + /* convert y to 1.15 format */ + return (y >> 20); + } + + + } + + /** + * + * @brief Process function for the Q7 Linear Interpolation Function. + * @param[in] *pYData pointer to Q7 Linear Interpolation table + * @param[in] x input sample to process + * @param[in] nValues number of table values + * @return y processed output sample. + * + * \par + * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. + * This function can support maximum of table size 2^12. + */ + + + static __INLINE q7_t arm_linear_interp_q7( + q7_t * pYData, + q31_t x, + uint32_t nValues) + { + q31_t y; /* output */ + q7_t y0, y1; /* Nearest output values */ + q31_t fract; /* fractional part */ + uint32_t index; /* Index to read nearest output values */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + if (x < 0) + { + return (pYData[0]); + } + index = (x >> 20) & 0xfff; + + + if(index >= (nValues - 1)) + { + return (pYData[nValues - 1]); + } + else + { + + /* 20 bits for the fractional part */ + /* fract is in 12.20 format */ + fract = (x & 0x000FFFFF); + + /* Read two nearest output values from the index and are in 1.7(q7) format */ + y0 = pYData[index]; + y1 = pYData[index + 1u]; + + /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */ + y = ((y0 * (0xFFFFF - fract))); + + /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */ + y += (y1 * fract); + + /* convert y to 1.7(q7) format */ + return (y >> 20u); + + } + + } + /** + * @} end of LinearInterpolate group + */ + + /** + * @brief Fast approximation to the trigonometric sine function for floating-point data. + * @param[in] x input value in radians. + * @return sin(x). + */ + + float32_t arm_sin_f32( + float32_t x); + + /** + * @brief Fast approximation to the trigonometric sine function for Q31 data. + * @param[in] x Scaled input value in radians. + * @return sin(x). + */ + + q31_t arm_sin_q31( + q31_t x); + + /** + * @brief Fast approximation to the trigonometric sine function for Q15 data. + * @param[in] x Scaled input value in radians. + * @return sin(x). + */ + + q15_t arm_sin_q15( + q15_t x); + + /** + * @brief Fast approximation to the trigonometric cosine function for floating-point data. + * @param[in] x input value in radians. + * @return cos(x). + */ + + float32_t arm_cos_f32( + float32_t x); + + /** + * @brief Fast approximation to the trigonometric cosine function for Q31 data. + * @param[in] x Scaled input value in radians. + * @return cos(x). + */ + + q31_t arm_cos_q31( + q31_t x); + + /** + * @brief Fast approximation to the trigonometric cosine function for Q15 data. + * @param[in] x Scaled input value in radians. + * @return cos(x). + */ + + q15_t arm_cos_q15( + q15_t x); + + + /** + * @ingroup groupFastMath + */ + + + /** + * @defgroup SQRT Square Root + * + * Computes the square root of a number. + * There are separate functions for Q15, Q31, and floating-point data types. + * The square root function is computed using the Newton-Raphson algorithm. + * This is an iterative algorithm of the form: + * 
+   *      x1 = x0 - f(x0)/f'(x0)
+   *
+ * where x1 is the current estimate, + * x0 is the previous estimate, and + * f'(x0) is the derivative of f() evaluated at x0. + * For the square root function, the algorithm reduces to: + * 
+   *     x0 = in/2                         [initial guess]
+   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]
+   *
+ */ + + + /** + * @addtogroup SQRT + * @{ + */ + + /** + * @brief Floating-point square root function. + * @param[in] in input value. + * @param[out] *pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * in is negative value and returns zero output for negative values. + */ + + static __INLINE arm_status arm_sqrt_f32( + float32_t in, + float32_t * pOut) + { + if(in > 0) + { + +// #if __FPU_USED +#if (__FPU_USED == 1) && defined ( __CC_ARM ) + *pOut = __sqrtf(in); +#else + *pOut = sqrtf(in); +#endif + + return (ARM_MATH_SUCCESS); + } + else + { + *pOut = 0.0f; + return (ARM_MATH_ARGUMENT_ERROR); + } + + } + + + /** + * @brief Q31 square root function. + * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF. + * @param[out] *pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * in is negative value and returns zero output for negative values. + */ + arm_status arm_sqrt_q31( + q31_t in, + q31_t * pOut); + + /** + * @brief Q15 square root function. + * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF. + * @param[out] *pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * in is negative value and returns zero output for negative values. + */ + arm_status arm_sqrt_q15( + q15_t in, + q15_t * pOut); + + /** + * @} end of SQRT group + */ + + + + + + + /** + * @brief floating-point Circular write function. + */ + + static __INLINE void arm_circularWrite_f32( + int32_t * circBuffer, + int32_t L, + uint16_t * writeOffset, + int32_t bufferInc, + const int32_t * src, + int32_t srcInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t wOffset; + + /* Copy the value of Index pointer that points + * to the current location where the input samples to be copied */ + wOffset = *writeOffset; + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the input sample to the circular buffer */ + circBuffer[wOffset] = *src; + + /* Update the input pointer */ + src += srcInc; + + /* Circularly update wOffset. Watch out for positive and negative value */ + wOffset += bufferInc; + if(wOffset >= L) + wOffset -= L; + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *writeOffset = wOffset; + } + + + + /** + * @brief floating-point Circular Read function. + */ + static __INLINE void arm_circularRead_f32( + int32_t * circBuffer, + int32_t L, + int32_t * readOffset, + int32_t bufferInc, + int32_t * dst, + int32_t * dst_base, + int32_t dst_length, + int32_t dstInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t rOffset, dst_end; + + /* Copy the value of Index pointer that points + * to the current location from where the input samples to be read */ + rOffset = *readOffset; + dst_end = (int32_t) (dst_base + dst_length); + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the sample from the circular buffer to the destination buffer */ + *dst = circBuffer[rOffset]; + + /* Update the input pointer */ + dst += dstInc; + + if(dst == (int32_t *) dst_end) + { + dst = dst_base; + } + + /* Circularly update rOffset. Watch out for positive and negative value */ + rOffset += bufferInc; + + if(rOffset >= L) + { + rOffset -= L; + } + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *readOffset = rOffset; + } + + /** + * @brief Q15 Circular write function. + */ + + static __INLINE void arm_circularWrite_q15( + q15_t * circBuffer, + int32_t L, + uint16_t * writeOffset, + int32_t bufferInc, + const q15_t * src, + int32_t srcInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t wOffset; + + /* Copy the value of Index pointer that points + * to the current location where the input samples to be copied */ + wOffset = *writeOffset; + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the input sample to the circular buffer */ + circBuffer[wOffset] = *src; + + /* Update the input pointer */ + src += srcInc; + + /* Circularly update wOffset. Watch out for positive and negative value */ + wOffset += bufferInc; + if(wOffset >= L) + wOffset -= L; + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *writeOffset = wOffset; + } + + + + /** + * @brief Q15 Circular Read function. + */ + static __INLINE void arm_circularRead_q15( + q15_t * circBuffer, + int32_t L, + int32_t * readOffset, + int32_t bufferInc, + q15_t * dst, + q15_t * dst_base, + int32_t dst_length, + int32_t dstInc, + uint32_t blockSize) + { + uint32_t i = 0; + int32_t rOffset, dst_end; + + /* Copy the value of Index pointer that points + * to the current location from where the input samples to be read */ + rOffset = *readOffset; + + dst_end = (int32_t) (dst_base + dst_length); + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the sample from the circular buffer to the destination buffer */ + *dst = circBuffer[rOffset]; + + /* Update the input pointer */ + dst += dstInc; + + if(dst == (q15_t *) dst_end) + { + dst = dst_base; + } + + /* Circularly update wOffset. Watch out for positive and negative value */ + rOffset += bufferInc; + + if(rOffset >= L) + { + rOffset -= L; + } + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *readOffset = rOffset; + } + + + /** + * @brief Q7 Circular write function. + */ + + static __INLINE void arm_circularWrite_q7( + q7_t * circBuffer, + int32_t L, + uint16_t * writeOffset, + int32_t bufferInc, + const q7_t * src, + int32_t srcInc, + uint32_t blockSize) + { + uint32_t i = 0u; + int32_t wOffset; + + /* Copy the value of Index pointer that points + * to the current location where the input samples to be copied */ + wOffset = *writeOffset; + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the input sample to the circular buffer */ + circBuffer[wOffset] = *src; + + /* Update the input pointer */ + src += srcInc; + + /* Circularly update wOffset. Watch out for positive and negative value */ + wOffset += bufferInc; + if(wOffset >= L) + wOffset -= L; + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *writeOffset = wOffset; + } + + + + /** + * @brief Q7 Circular Read function. + */ + static __INLINE void arm_circularRead_q7( + q7_t * circBuffer, + int32_t L, + int32_t * readOffset, + int32_t bufferInc, + q7_t * dst, + q7_t * dst_base, + int32_t dst_length, + int32_t dstInc, + uint32_t blockSize) + { + uint32_t i = 0; + int32_t rOffset, dst_end; + + /* Copy the value of Index pointer that points + * to the current location from where the input samples to be read */ + rOffset = *readOffset; + + dst_end = (int32_t) (dst_base + dst_length); + + /* Loop over the blockSize */ + i = blockSize; + + while(i > 0u) + { + /* copy the sample from the circular buffer to the destination buffer */ + *dst = circBuffer[rOffset]; + + /* Update the input pointer */ + dst += dstInc; + + if(dst == (q7_t *) dst_end) + { + dst = dst_base; + } + + /* Circularly update rOffset. Watch out for positive and negative value */ + rOffset += bufferInc; + + if(rOffset >= L) + { + rOffset -= L; + } + + /* Decrement the loop counter */ + i--; + } + + /* Update the index pointer */ + *readOffset = rOffset; + } + + + /** + * @brief Sum of the squares of the elements of a Q31 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_power_q31( + q31_t * pSrc, + uint32_t blockSize, + q63_t * pResult); + + /** + * @brief Sum of the squares of the elements of a floating-point vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_power_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + /** + * @brief Sum of the squares of the elements of a Q15 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_power_q15( + q15_t * pSrc, + uint32_t blockSize, + q63_t * pResult); + + /** + * @brief Sum of the squares of the elements of a Q7 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_power_q7( + q7_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + /** + * @brief Mean value of a Q7 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_mean_q7( + q7_t * pSrc, + uint32_t blockSize, + q7_t * pResult); + + /** + * @brief Mean value of a Q15 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + void arm_mean_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + /** + * @brief Mean value of a Q31 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + void arm_mean_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + /** + * @brief Mean value of a floating-point vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + void arm_mean_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + /** + * @brief Variance of the elements of a floating-point vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_var_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + /** + * @brief Variance of the elements of a Q31 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_var_q31( + q31_t * pSrc, + uint32_t blockSize, + q63_t * pResult); + + /** + * @brief Variance of the elements of a Q15 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_var_q15( + q15_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + /** + * @brief Root Mean Square of the elements of a floating-point vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_rms_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + /** + * @brief Root Mean Square of the elements of a Q31 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_rms_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + /** + * @brief Root Mean Square of the elements of a Q15 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_rms_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + /** + * @brief Standard deviation of the elements of a floating-point vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_std_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult); + + /** + * @brief Standard deviation of the elements of a Q31 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_std_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult); + + /** + * @brief Standard deviation of the elements of a Q15 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output value. + * @return none. + */ + + void arm_std_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult); + + /** + * @brief Floating-point complex magnitude + * @param[in] *pSrc points to the complex input vector + * @param[out] *pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + * @return none. + */ + + void arm_cmplx_mag_f32( + float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples); + + /** + * @brief Q31 complex magnitude + * @param[in] *pSrc points to the complex input vector + * @param[out] *pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + * @return none. + */ + + void arm_cmplx_mag_q31( + q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples); + + /** + * @brief Q15 complex magnitude + * @param[in] *pSrc points to the complex input vector + * @param[out] *pDst points to the real output vector + * @param[in] numSamples number of complex samples in the input vector + * @return none. + */ + + void arm_cmplx_mag_q15( + q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples); + + /** + * @brief Q15 complex dot product + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] numSamples number of complex samples in each vector + * @param[out] *realResult real part of the result returned here + * @param[out] *imagResult imaginary part of the result returned here + * @return none. + */ + + void arm_cmplx_dot_prod_q15( + q15_t * pSrcA, + q15_t * pSrcB, + uint32_t numSamples, + q31_t * realResult, + q31_t * imagResult); + + /** + * @brief Q31 complex dot product + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] numSamples number of complex samples in each vector + * @param[out] *realResult real part of the result returned here + * @param[out] *imagResult imaginary part of the result returned here + * @return none. + */ + + void arm_cmplx_dot_prod_q31( + q31_t * pSrcA, + q31_t * pSrcB, + uint32_t numSamples, + q63_t * realResult, + q63_t * imagResult); + + /** + * @brief Floating-point complex dot product + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[in] numSamples number of complex samples in each vector + * @param[out] *realResult real part of the result returned here + * @param[out] *imagResult imaginary part of the result returned here + * @return none. + */ + + void arm_cmplx_dot_prod_f32( + float32_t * pSrcA, + float32_t * pSrcB, + uint32_t numSamples, + float32_t * realResult, + float32_t * imagResult); + + /** + * @brief Q15 complex-by-real multiplication + * @param[in] *pSrcCmplx points to the complex input vector + * @param[in] *pSrcReal points to the real input vector + * @param[out] *pCmplxDst points to the complex output vector + * @param[in] numSamples number of samples in each vector + * @return none. + */ + + void arm_cmplx_mult_real_q15( + q15_t * pSrcCmplx, + q15_t * pSrcReal, + q15_t * pCmplxDst, + uint32_t numSamples); + + /** + * @brief Q31 complex-by-real multiplication + * @param[in] *pSrcCmplx points to the complex input vector + * @param[in] *pSrcReal points to the real input vector + * @param[out] *pCmplxDst points to the complex output vector + * @param[in] numSamples number of samples in each vector + * @return none. + */ + + void arm_cmplx_mult_real_q31( + q31_t * pSrcCmplx, + q31_t * pSrcReal, + q31_t * pCmplxDst, + uint32_t numSamples); + + /** + * @brief Floating-point complex-by-real multiplication + * @param[in] *pSrcCmplx points to the complex input vector + * @param[in] *pSrcReal points to the real input vector + * @param[out] *pCmplxDst points to the complex output vector + * @param[in] numSamples number of samples in each vector + * @return none. + */ + + void arm_cmplx_mult_real_f32( + float32_t * pSrcCmplx, + float32_t * pSrcReal, + float32_t * pCmplxDst, + uint32_t numSamples); + + /** + * @brief Minimum value of a Q7 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *result is output pointer + * @param[in] index is the array index of the minimum value in the input buffer. + * @return none. + */ + + void arm_min_q7( + q7_t * pSrc, + uint32_t blockSize, + q7_t * result, + uint32_t * index); + + /** + * @brief Minimum value of a Q15 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output pointer + * @param[in] *pIndex is the array index of the minimum value in the input buffer. + * @return none. + */ + + void arm_min_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult, + uint32_t * pIndex); + + /** + * @brief Minimum value of a Q31 vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output pointer + * @param[out] *pIndex is the array index of the minimum value in the input buffer. + * @return none. + */ + void arm_min_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult, + uint32_t * pIndex); + + /** + * @brief Minimum value of a floating-point vector. + * @param[in] *pSrc is input pointer + * @param[in] blockSize is the number of samples to process + * @param[out] *pResult is output pointer + * @param[out] *pIndex is the array index of the minimum value in the input buffer. + * @return none. + */ + + void arm_min_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult, + uint32_t * pIndex); + +/** + * @brief Maximum value of a Q7 vector. + * @param[in] *pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] *pResult maximum value returned here + * @param[out] *pIndex index of maximum value returned here + * @return none. + */ + + void arm_max_q7( + q7_t * pSrc, + uint32_t blockSize, + q7_t * pResult, + uint32_t * pIndex); + +/** + * @brief Maximum value of a Q15 vector. + * @param[in] *pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] *pResult maximum value returned here + * @param[out] *pIndex index of maximum value returned here + * @return none. + */ + + void arm_max_q15( + q15_t * pSrc, + uint32_t blockSize, + q15_t * pResult, + uint32_t * pIndex); + +/** + * @brief Maximum value of a Q31 vector. + * @param[in] *pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] *pResult maximum value returned here + * @param[out] *pIndex index of maximum value returned here + * @return none. + */ + + void arm_max_q31( + q31_t * pSrc, + uint32_t blockSize, + q31_t * pResult, + uint32_t * pIndex); + +/** + * @brief Maximum value of a floating-point vector. + * @param[in] *pSrc points to the input buffer + * @param[in] blockSize length of the input vector + * @param[out] *pResult maximum value returned here + * @param[out] *pIndex index of maximum value returned here + * @return none. + */ + + void arm_max_f32( + float32_t * pSrc, + uint32_t blockSize, + float32_t * pResult, + uint32_t * pIndex); + + /** + * @brief Q15 complex-by-complex multiplication + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + * @return none. + */ + + void arm_cmplx_mult_cmplx_q15( + q15_t * pSrcA, + q15_t * pSrcB, + q15_t * pDst, + uint32_t numSamples); + + /** + * @brief Q31 complex-by-complex multiplication + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + * @return none. + */ + + void arm_cmplx_mult_cmplx_q31( + q31_t * pSrcA, + q31_t * pSrcB, + q31_t * pDst, + uint32_t numSamples); + + /** + * @brief Floating-point complex-by-complex multiplication + * @param[in] *pSrcA points to the first input vector + * @param[in] *pSrcB points to the second input vector + * @param[out] *pDst points to the output vector + * @param[in] numSamples number of complex samples in each vector + * @return none. + */ + + void arm_cmplx_mult_cmplx_f32( + float32_t * pSrcA, + float32_t * pSrcB, + float32_t * pDst, + uint32_t numSamples); + + /** + * @brief Converts the elements of the floating-point vector to Q31 vector. + * @param[in] *pSrc points to the floating-point input vector + * @param[out] *pDst points to the Q31 output vector + * @param[in] blockSize length of the input vector + * @return none. + */ + void arm_float_to_q31( + float32_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + /** + * @brief Converts the elements of the floating-point vector to Q15 vector. + * @param[in] *pSrc points to the floating-point input vector + * @param[out] *pDst points to the Q15 output vector + * @param[in] blockSize length of the input vector + * @return none + */ + void arm_float_to_q15( + float32_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Converts the elements of the floating-point vector to Q7 vector. + * @param[in] *pSrc points to the floating-point input vector + * @param[out] *pDst points to the Q7 output vector + * @param[in] blockSize length of the input vector + * @return none + */ + void arm_float_to_q7( + float32_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q31 vector to Q15 vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q31_to_q15( + q31_t * pSrc, + q15_t * pDst, + uint32_t blockSize); + + /** + * @brief Converts the elements of the Q31 vector to Q7 vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q31_to_q7( + q31_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + /** + * @brief Converts the elements of the Q15 vector to floating-point vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q15_to_float( + q15_t * pSrc, + float32_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q15 vector to Q31 vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q15_to_q31( + q15_t * pSrc, + q31_t * pDst, + uint32_t blockSize); + + + /** + * @brief Converts the elements of the Q15 vector to Q7 vector. + * @param[in] *pSrc is input pointer + * @param[out] *pDst is output pointer + * @param[in] blockSize is the number of samples to process + * @return none. + */ + void arm_q15_to_q7( + q15_t * pSrc, + q7_t * pDst, + uint32_t blockSize); + + + /** + * @ingroup groupInterpolation + */ + + /** + * @defgroup BilinearInterpolate Bilinear Interpolation + * + * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid. + * The underlying function f(x, y) is sampled on a regular grid and the interpolation process + * determines values between the grid points. + * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension. + * Bilinear interpolation is often used in image processing to rescale images. + * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types. + * + * Algorithm + * \par + * The instance structure used by the bilinear interpolation functions describes a two dimensional data table. + * For floating-point, the instance structure is defined as: + * 
+   *   typedef struct
+   *   {
+   *     uint16_t numRows;
+   *     uint16_t numCols;
+   *     float32_t *pData;
+   * } arm_bilinear_interp_instance_f32;
+   *
+ * + * \par + * where numRows specifies the number of rows in the table; + * numCols specifies the number of columns in the table; + * and pData points to an array of size numRows*numCols values. + * The data table pTable is organized in row order and the supplied data values fall on integer indexes. + * That is, table element (x,y) is located at pTable[x + y*numCols] where x and y are integers. + * + * \par + * Let (x, y) specify the desired interpolation point. Then define: + * 
+   *     XF = floor(x)
+   *     YF = floor(y)
+   *
+ * \par + * The interpolated output point is computed as: + * 
+   *  f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
+   *           + f(XF+1, YF) * (x-XF)*(1-(y-YF))
+   *           + f(XF, YF+1) * (1-(x-XF))*(y-YF)
+   *           + f(XF+1, YF+1) * (x-XF)*(y-YF)
+   *
+ * Note that the coordinates (x, y) contain integer and fractional components. + * The integer components specify which portion of the table to use while the + * fractional components control the interpolation processor. + * + * \par + * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output. + */ + + /** + * @addtogroup BilinearInterpolate + * @{ + */ + + /** + * + * @brief Floating-point bilinear interpolation. + * @param[in,out] *S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate. + * @param[in] Y interpolation coordinate. + * @return out interpolated value. + */ + + + static __INLINE float32_t arm_bilinear_interp_f32( + const arm_bilinear_interp_instance_f32 * S, + float32_t X, + float32_t Y) + { + float32_t out; + float32_t f00, f01, f10, f11; + float32_t *pData = S->pData; + int32_t xIndex, yIndex, index; + float32_t xdiff, ydiff; + float32_t b1, b2, b3, b4; + + xIndex = (int32_t) X; + yIndex = (int32_t) Y; + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0 + || yIndex > (S->numCols - 1)) + { + return (0); + } + + /* Calculation of index for two nearest points in X-direction */ + index = (xIndex - 1) + (yIndex - 1) * S->numCols; + + + /* Read two nearest points in X-direction */ + f00 = pData[index]; + f01 = pData[index + 1]; + + /* Calculation of index for two nearest points in Y-direction */ + index = (xIndex - 1) + (yIndex) * S->numCols; + + + /* Read two nearest points in Y-direction */ + f10 = pData[index]; + f11 = pData[index + 1]; + + /* Calculation of intermediate values */ + b1 = f00; + b2 = f01 - f00; + b3 = f10 - f00; + b4 = f00 - f01 - f10 + f11; + + /* Calculation of fractional part in X */ + xdiff = X - xIndex; + + /* Calculation of fractional part in Y */ + ydiff = Y - yIndex; + + /* Calculation of bi-linear interpolated output */ + out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff; + + /* return to application */ + return (out); + + } + + /** + * + * @brief Q31 bilinear interpolation. + * @param[in,out] *S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate in 12.20 format. + * @param[in] Y interpolation coordinate in 12.20 format. + * @return out interpolated value. + */ + + static __INLINE q31_t arm_bilinear_interp_q31( + arm_bilinear_interp_instance_q31 * S, + q31_t X, + q31_t Y) + { + q31_t out; /* Temporary output */ + q31_t acc = 0; /* output */ + q31_t xfract, yfract; /* X, Y fractional parts */ + q31_t x1, x2, y1, y2; /* Nearest output values */ + int32_t rI, cI; /* Row and column indices */ + q31_t *pYData = S->pData; /* pointer to output table values */ + uint32_t nCols = S->numCols; /* num of rows */ + + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + rI = ((X & 0xFFF00000) >> 20u); + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + cI = ((Y & 0xFFF00000) >> 20u); + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) + { + return (0); + } + + /* 20 bits for the fractional part */ + /* shift left xfract by 11 to keep 1.31 format */ + xfract = (X & 0x000FFFFF) << 11u; + + /* Read two nearest output values from the index */ + x1 = pYData[(rI) + nCols * (cI)]; + x2 = pYData[(rI) + nCols * (cI) + 1u]; + + /* 20 bits for the fractional part */ + /* shift left yfract by 11 to keep 1.31 format */ + yfract = (Y & 0x000FFFFF) << 11u; + + /* Read two nearest output values from the index */ + y1 = pYData[(rI) + nCols * (cI + 1)]; + y2 = pYData[(rI) + nCols * (cI + 1) + 1u]; + + /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */ + out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32)); + acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32)); + + /* x2 * (xfract) * (1-yfract) in 3.29(q29) and adding to acc */ + out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32)); + acc += ((q31_t) ((q63_t) out * (xfract) >> 32)); + + /* y1 * (1 - xfract) * (yfract) in 3.29(q29) and adding to acc */ + out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32)); + acc += ((q31_t) ((q63_t) out * (yfract) >> 32)); + + /* y2 * (xfract) * (yfract) in 3.29(q29) and adding to acc */ + out = ((q31_t) ((q63_t) y2 * (xfract) >> 32)); + acc += ((q31_t) ((q63_t) out * (yfract) >> 32)); + + /* Convert acc to 1.31(q31) format */ + return (acc << 2u); + + } + + /** + * @brief Q15 bilinear interpolation. + * @param[in,out] *S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate in 12.20 format. + * @param[in] Y interpolation coordinate in 12.20 format. + * @return out interpolated value. + */ + + static __INLINE q15_t arm_bilinear_interp_q15( + arm_bilinear_interp_instance_q15 * S, + q31_t X, + q31_t Y) + { + q63_t acc = 0; /* output */ + q31_t out; /* Temporary output */ + q15_t x1, x2, y1, y2; /* Nearest output values */ + q31_t xfract, yfract; /* X, Y fractional parts */ + int32_t rI, cI; /* Row and column indices */ + q15_t *pYData = S->pData; /* pointer to output table values */ + uint32_t nCols = S->numCols; /* num of rows */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + rI = ((X & 0xFFF00000) >> 20); + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + cI = ((Y & 0xFFF00000) >> 20); + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) + { + return (0); + } + + /* 20 bits for the fractional part */ + /* xfract should be in 12.20 format */ + xfract = (X & 0x000FFFFF); + + /* Read two nearest output values from the index */ + x1 = pYData[(rI) + nCols * (cI)]; + x2 = pYData[(rI) + nCols * (cI) + 1u]; + + + /* 20 bits for the fractional part */ + /* yfract should be in 12.20 format */ + yfract = (Y & 0x000FFFFF); + + /* Read two nearest output values from the index */ + y1 = pYData[(rI) + nCols * (cI + 1)]; + y2 = pYData[(rI) + nCols * (cI + 1) + 1u]; + + /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */ + + /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */ + /* convert 13.35 to 13.31 by right shifting and out is in 1.31 */ + out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u); + acc = ((q63_t) out * (0xFFFFF - yfract)); + + /* x2 * (xfract) * (1-yfract) in 1.51 and adding to acc */ + out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u); + acc += ((q63_t) out * (xfract)); + + /* y1 * (1 - xfract) * (yfract) in 1.51 and adding to acc */ + out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u); + acc += ((q63_t) out * (yfract)); + + /* y2 * (xfract) * (yfract) in 1.51 and adding to acc */ + out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u); + acc += ((q63_t) out * (yfract)); + + /* acc is in 13.51 format and down shift acc by 36 times */ + /* Convert out to 1.15 format */ + return (acc >> 36); + + } + + /** + * @brief Q7 bilinear interpolation. + * @param[in,out] *S points to an instance of the interpolation structure. + * @param[in] X interpolation coordinate in 12.20 format. + * @param[in] Y interpolation coordinate in 12.20 format. + * @return out interpolated value. + */ + + static __INLINE q7_t arm_bilinear_interp_q7( + arm_bilinear_interp_instance_q7 * S, + q31_t X, + q31_t Y) + { + q63_t acc = 0; /* output */ + q31_t out; /* Temporary output */ + q31_t xfract, yfract; /* X, Y fractional parts */ + q7_t x1, x2, y1, y2; /* Nearest output values */ + int32_t rI, cI; /* Row and column indices */ + q7_t *pYData = S->pData; /* pointer to output table values */ + uint32_t nCols = S->numCols; /* num of rows */ + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + rI = ((X & 0xFFF00000) >> 20); + + /* Input is in 12.20 format */ + /* 12 bits for the table index */ + /* Index value calculation */ + cI = ((Y & 0xFFF00000) >> 20); + + /* Care taken for table outside boundary */ + /* Returns zero output when values are outside table boundary */ + if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) + { + return (0); + } + + /* 20 bits for the fractional part */ + /* xfract should be in 12.20 format */ + xfract = (X & 0x000FFFFF); + + /* Read two nearest output values from the index */ + x1 = pYData[(rI) + nCols * (cI)]; + x2 = pYData[(rI) + nCols * (cI) + 1u]; + + + /* 20 bits for the fractional part */ + /* yfract should be in 12.20 format */ + yfract = (Y & 0x000FFFFF); + + /* Read two nearest output values from the index */ + y1 = pYData[(rI) + nCols * (cI + 1)]; + y2 = pYData[(rI) + nCols * (cI + 1) + 1u]; + + /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */ + out = ((x1 * (0xFFFFF - xfract))); + acc = (((q63_t) out * (0xFFFFF - yfract))); + + /* x2 * (xfract) * (1-yfract) in 2.22 and adding to acc */ + out = ((x2 * (0xFFFFF - yfract))); + acc += (((q63_t) out * (xfract))); + + /* y1 * (1 - xfract) * (yfract) in 2.22 and adding to acc */ + out = ((y1 * (0xFFFFF - xfract))); + acc += (((q63_t) out * (yfract))); + + /* y2 * (xfract) * (yfract) in 2.22 and adding to acc */ + out = ((y2 * (yfract))); + acc += (((q63_t) out * (xfract))); + + /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */ + return (acc >> 40); + + } + + /** + * @} end of BilinearInterpolate group + */ + + +#if defined ( __CC_ARM ) //Keil +//SMMLAR + #define multAcc_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32) + +//SMMLSR + #define multSub_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32) + +//SMMULR + #define mult_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32) + +//Enter low optimization region - place directly above function definition + #define LOW_OPTIMIZATION_ENTER \ + _Pragma ("push") \ + _Pragma ("O1") + +//Exit low optimization region - place directly after end of function definition + #define LOW_OPTIMIZATION_EXIT \ + _Pragma ("pop") + +//Enter low optimization region - place directly above function definition + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + +//Exit low optimization region - place directly after end of function definition + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__ICCARM__) //IAR + //SMMLA + #define multAcc_32x32_keep32_R(a, x, y) \ + a += (q31_t) (((q63_t) x * y) >> 32) + + //SMMLS + #define multSub_32x32_keep32_R(a, x, y) \ + a -= (q31_t) (((q63_t) x * y) >> 32) + +//SMMUL + #define mult_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((q63_t) x * y ) >> 32) + +//Enter low optimization region - place directly above function definition + #define LOW_OPTIMIZATION_ENTER \ + _Pragma ("optimize=low") + +//Exit low optimization region - place directly after end of function definition + #define LOW_OPTIMIZATION_EXIT + +//Enter low optimization region - place directly above function definition + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \ + _Pragma ("optimize=low") + +//Exit low optimization region - place directly after end of function definition + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#elif defined(__GNUC__) + //SMMLA + #define multAcc_32x32_keep32_R(a, x, y) \ + a += (q31_t) (((q63_t) x * y) >> 32) + + //SMMLS + #define multSub_32x32_keep32_R(a, x, y) \ + a -= (q31_t) (((q63_t) x * y) >> 32) + +//SMMUL + #define mult_32x32_keep32_R(a, x, y) \ + a = (q31_t) (((q63_t) x * y ) >> 32) + + #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") )) + + #define LOW_OPTIMIZATION_EXIT + + #define IAR_ONLY_LOW_OPTIMIZATION_ENTER + + #define IAR_ONLY_LOW_OPTIMIZATION_EXIT + +#endif + + + + + +#ifdef __cplusplus +} +#endif + + +#endif /* _ARM_MATH_H */ + + +/** + * + * End of file. + */ diff --git a/src/lib/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h new file mode 100644 index 000000000..8ac6dc078 --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h @@ -0,0 +1,1627 @@ +/**************************************************************************//** + * @file core_cm3.h + * @brief CMSIS Cortex-M3 Core Peripheral Access Layer Header File + * @version V3.20 + * @date 25. February 2013 + * + * @note + * + ******************************************************************************/ +/* Copyright (c) 2009 - 2013 ARM LIMITED + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - 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. + - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. + ---------------------------------------------------------------------------*/ + + +#if defined ( __ICCARM__ ) + #pragma system_include /* treat file as system include file for MISRA check */ +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +#ifndef __CORE_CM3_H_GENERIC +#define __CORE_CM3_H_GENERIC + +/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions + CMSIS violates the following MISRA-C:2004 rules: + + \li Required Rule 8.5, object/function definition in header file.
+ Function definitions in header files are used to allow 'inlining'. + + \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.
+ Unions are used for effective representation of core registers. + + \li Advisory Rule 19.7, Function-like macro defined.
+ Function-like macros are used to allow more efficient code. + */ + + +/******************************************************************************* + * CMSIS definitions + ******************************************************************************/ +/** \ingroup Cortex_M3 + @{ + */ + +/* CMSIS CM3 definitions */ +#define __CM3_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */ +#define __CM3_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */ +#define __CM3_CMSIS_VERSION ((__CM3_CMSIS_VERSION_MAIN << 16) | \ + __CM3_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */ + +#define __CORTEX_M (0x03) /*!< Cortex-M Core */ + + +#if defined ( __CC_ARM ) + #define __ASM __asm /*!< asm keyword for ARM Compiler */ + #define __INLINE __inline /*!< inline keyword for ARM Compiler */ + #define __STATIC_INLINE static __inline + +#elif defined ( __ICCARM__ ) + #define __ASM __asm /*!< asm keyword for IAR Compiler */ + #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */ + #define __STATIC_INLINE static inline + +#elif defined ( __TMS470__ ) + #define __ASM __asm /*!< asm keyword for TI CCS Compiler */ + #define __STATIC_INLINE static inline + +#elif defined ( __GNUC__ ) + #define __ASM __asm /*!< asm keyword for GNU Compiler */ + #define __INLINE inline /*!< inline keyword for GNU Compiler */ + #define __STATIC_INLINE static inline + +#elif defined ( __TASKING__ ) + #define __ASM __asm /*!< asm keyword for TASKING Compiler */ + #define __INLINE inline /*!< inline keyword for TASKING Compiler */ + #define __STATIC_INLINE static inline + +#endif + +/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all +*/ +#define __FPU_USED 0 + +#if defined ( __CC_ARM ) + #if defined __TARGET_FPU_VFP + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #endif + +#elif defined ( __ICCARM__ ) + #if defined __ARMVFP__ + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #endif + +#elif defined ( __TMS470__ ) + #if defined __TI__VFP_SUPPORT____ + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #endif + +#elif defined ( __GNUC__ ) + #if defined (__VFP_FP__) && !defined(__SOFTFP__) + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #endif + +#elif defined ( __TASKING__ ) + #if defined __FPU_VFP__ + #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #endif +#endif + +#include /* standard types definitions */ +#include "core_cmInstr.h" /* Core Instruction Access */ +#include "core_cmFunc.h" /* Core Function Access */ + +#endif /* __CORE_CM3_H_GENERIC */ + +#ifndef __CMSIS_GENERIC + +#ifndef __CORE_CM3_H_DEPENDANT +#define __CORE_CM3_H_DEPENDANT + +/* check device defines and use defaults */ +#if defined __CHECK_DEVICE_DEFINES + #ifndef __CM3_REV + #define __CM3_REV 0x0200 + #warning "__CM3_REV not defined in device header file; using default!" + #endif + + #ifndef __MPU_PRESENT + #define __MPU_PRESENT 0 + #warning "__MPU_PRESENT not defined in device header file; using default!" + #endif + + #ifndef __NVIC_PRIO_BITS + #define __NVIC_PRIO_BITS 4 + #warning "__NVIC_PRIO_BITS not defined in device header file; using default!" + #endif + + #ifndef __Vendor_SysTickConfig + #define __Vendor_SysTickConfig 0 + #warning "__Vendor_SysTickConfig not defined in device header file; using default!" + #endif +#endif + +/* IO definitions (access restrictions to peripheral registers) */ +/** + \defgroup CMSIS_glob_defs CMSIS Global Defines + + IO Type Qualifiers are used + \li to specify the access to peripheral variables. + \li for automatic generation of peripheral register debug information. +*/ +#ifdef __cplusplus + #define __I volatile /*!< Defines 'read only' permissions */ +#else + #define __I volatile const /*!< Defines 'read only' permissions */ +#endif +#define __O volatile /*!< Defines 'write only' permissions */ +#define __IO volatile /*!< Defines 'read / write' permissions */ + +/*@} end of group Cortex_M3 */ + + + +/******************************************************************************* + * Register Abstraction + Core Register contain: + - Core Register + - Core NVIC Register + - Core SCB Register + - Core SysTick Register + - Core Debug Register + - Core MPU Register + ******************************************************************************/ +/** \defgroup CMSIS_core_register Defines and Type Definitions + \brief Type definitions and defines for Cortex-M processor based devices. +*/ + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_CORE Status and Control Registers + \brief Core Register type definitions. + @{ + */ + +/** \brief Union type to access the Application Program Status Register (APSR). + */ +typedef union +{ + struct + { +#if (__CORTEX_M != 0x04) + uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */ +#else + uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */ + uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ + uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */ +#endif + uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ + uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ + uint32_t C:1; /*!< bit: 29 Carry condition code flag */ + uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ + uint32_t N:1; /*!< bit: 31 Negative condition code flag */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} APSR_Type; + + +/** \brief Union type to access the Interrupt Program Status Register (IPSR). + */ +typedef union +{ + struct + { + uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ + uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} IPSR_Type; + + +/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR). + */ +typedef union +{ + struct + { + uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ +#if (__CORTEX_M != 0x04) + uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */ +#else + uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */ + uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ + uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */ +#endif + uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */ + uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */ + uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ + uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ + uint32_t C:1; /*!< bit: 29 Carry condition code flag */ + uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ + uint32_t N:1; /*!< bit: 31 Negative condition code flag */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} xPSR_Type; + + +/** \brief Union type to access the Control Registers (CONTROL). + */ +typedef union +{ + struct + { + uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */ + uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */ + uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */ + uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} CONTROL_Type; + +/*@} end of group CMSIS_CORE */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC) + \brief Type definitions for the NVIC Registers + @{ + */ + +/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC). + */ +typedef struct +{ + __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ + uint32_t RESERVED0[24]; + __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ + uint32_t RSERVED1[24]; + __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ + uint32_t RESERVED2[24]; + __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ + uint32_t RESERVED3[24]; + __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ + uint32_t RESERVED4[56]; + __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ + uint32_t RESERVED5[644]; + __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ +} NVIC_Type; + +/* Software Triggered Interrupt Register Definitions */ +#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */ +#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */ + +/*@} end of group CMSIS_NVIC */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_SCB System Control Block (SCB) + \brief Type definitions for the System Control Block Registers + @{ + */ + +/** \brief Structure type to access the System Control Block (SCB). + */ +typedef struct +{ + __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */ + __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */ + __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */ + __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */ + __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */ + __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */ + __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */ + __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */ + __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */ + __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */ + __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */ + __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */ + __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */ + __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */ + __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */ + __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */ + __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */ + __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */ + __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */ + uint32_t RESERVED0[5]; + __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */ +} SCB_Type; + +/* SCB CPUID Register Definitions */ +#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */ +#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */ + +#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */ +#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */ + +#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */ +#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */ + +#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */ +#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */ + +#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */ +#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */ + +/* SCB Interrupt Control State Register Definitions */ +#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */ +#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */ + +#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */ +#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */ + +#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */ +#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */ + +#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */ +#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */ + +#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */ +#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */ + +#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */ +#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */ + +#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */ +#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */ + +#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */ +#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */ + +#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */ +#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */ + +#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */ +#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */ + +/* SCB Vector Table Offset Register Definitions */ +#if (__CM3_REV < 0x0201) /* core r2p1 */ +#define SCB_VTOR_TBLBASE_Pos 29 /*!< SCB VTOR: TBLBASE Position */ +#define SCB_VTOR_TBLBASE_Msk (1UL << SCB_VTOR_TBLBASE_Pos) /*!< SCB VTOR: TBLBASE Mask */ + +#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */ +#define SCB_VTOR_TBLOFF_Msk (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ +#else +#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */ +#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ +#endif + +/* SCB Application Interrupt and Reset Control Register Definitions */ +#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */ +#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */ + +#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */ +#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */ + +#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */ +#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */ + +#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */ +#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ + +#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */ +#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */ + +#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */ +#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */ + +#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */ +#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */ + +/* SCB System Control Register Definitions */ +#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */ +#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */ + +#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */ +#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */ + +#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */ +#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */ + +/* SCB Configuration Control Register Definitions */ +#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */ +#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */ + +#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */ +#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */ + +#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */ +#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */ + +#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */ +#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */ + +#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */ +#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */ + +#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */ +#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */ + +/* SCB System Handler Control and State Register Definitions */ +#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */ +#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */ + +#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */ +#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */ + +#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */ +#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */ + +#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */ +#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */ + +#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */ +#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */ + +#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */ +#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */ + +#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */ +#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */ + +#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */ +#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */ + +#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */ +#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */ + +#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */ +#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */ + +#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */ +#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */ + +#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */ +#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */ + +#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */ +#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */ + +#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */ +#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */ + +/* SCB Configurable Fault Status Registers Definitions */ +#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */ +#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */ + +#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */ +#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */ + +#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */ +#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */ + +/* SCB Hard Fault Status Registers Definitions */ +#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */ +#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */ + +#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */ +#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */ + +#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */ +#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */ + +/* SCB Debug Fault Status Register Definitions */ +#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */ +#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */ + +#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */ +#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */ + +#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */ +#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */ + +#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */ +#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */ + +#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */ +#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */ + +/*@} end of group CMSIS_SCB */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB) + \brief Type definitions for the System Control and ID Register not in the SCB + @{ + */ + +/** \brief Structure type to access the System Control and ID Register not in the SCB. + */ +typedef struct +{ + uint32_t RESERVED0[1]; + __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */ +#if ((defined __CM3_REV) && (__CM3_REV >= 0x200)) + __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */ +#else + uint32_t RESERVED1[1]; +#endif +} SCnSCB_Type; + +/* Interrupt Controller Type Register Definitions */ +#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */ +#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */ + +/* Auxiliary Control Register Definitions */ + +#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */ +#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */ + +#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */ +#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */ + +#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */ +#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */ + +/*@} end of group CMSIS_SCnotSCB */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_SysTick System Tick Timer (SysTick) + \brief Type definitions for the System Timer Registers. + @{ + */ + +/** \brief Structure type to access the System Timer (SysTick). + */ +typedef struct +{ + __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */ + __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */ + __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */ + __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */ +} SysTick_Type; + +/* SysTick Control / Status Register Definitions */ +#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */ +#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */ + +#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */ +#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */ + +#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */ +#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */ + +#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */ +#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */ + +/* SysTick Reload Register Definitions */ +#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */ +#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */ + +/* SysTick Current Register Definitions */ +#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */ +#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */ + +/* SysTick Calibration Register Definitions */ +#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */ +#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */ + +#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */ +#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */ + +#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */ +#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */ + +/*@} end of group CMSIS_SysTick */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM) + \brief Type definitions for the Instrumentation Trace Macrocell (ITM) + @{ + */ + +/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM). + */ +typedef struct +{ + __O union + { + __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */ + __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */ + __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */ + } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */ + uint32_t RESERVED0[864]; + __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */ + uint32_t RESERVED1[15]; + __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */ + uint32_t RESERVED2[15]; + __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */ + uint32_t RESERVED3[29]; + __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */ + __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */ + __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */ + uint32_t RESERVED4[43]; + __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */ + __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */ + uint32_t RESERVED5[6]; + __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */ + __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */ + __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */ + __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */ + __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */ + __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */ + __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */ + __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */ + __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */ + __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */ + __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */ + __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */ +} ITM_Type; + +/* ITM Trace Privilege Register Definitions */ +#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */ +#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */ + +/* ITM Trace Control Register Definitions */ +#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */ +#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */ + +#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */ +#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */ + +#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */ +#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */ + +#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */ +#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */ + +#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */ +#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */ + +#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */ +#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */ + +#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */ +#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */ + +#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */ +#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */ + +#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */ +#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */ + +/* ITM Integration Write Register Definitions */ +#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */ +#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */ + +/* ITM Integration Read Register Definitions */ +#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */ +#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */ + +/* ITM Integration Mode Control Register Definitions */ +#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */ +#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */ + +/* ITM Lock Status Register Definitions */ +#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */ +#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */ + +#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */ +#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */ + +#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */ +#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */ + +/*@}*/ /* end of group CMSIS_ITM */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT) + \brief Type definitions for the Data Watchpoint and Trace (DWT) + @{ + */ + +/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT). + */ +typedef struct +{ + __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */ + __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */ + __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */ + __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */ + __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */ + __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */ + __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */ + __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */ + __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */ + __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */ + __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */ + uint32_t RESERVED0[1]; + __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */ + __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */ + __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */ + uint32_t RESERVED1[1]; + __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */ + __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */ + __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */ + uint32_t RESERVED2[1]; + __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */ + __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */ + __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */ +} DWT_Type; + +/* DWT Control Register Definitions */ +#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */ +#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */ + +#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */ +#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */ + +#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */ +#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */ + +#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */ +#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */ + +#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */ +#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */ + +#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */ +#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */ + +#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */ +#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */ + +#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */ +#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */ + +#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */ +#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */ + +#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */ +#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */ + +#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */ +#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */ + +#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */ +#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */ + +#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */ +#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */ + +#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */ +#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */ + +#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */ +#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */ + +#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */ +#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */ + +#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */ +#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */ + +#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */ +#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */ + +/* DWT CPI Count Register Definitions */ +#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */ +#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */ + +/* DWT Exception Overhead Count Register Definitions */ +#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */ +#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */ + +/* DWT Sleep Count Register Definitions */ +#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */ +#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */ + +/* DWT LSU Count Register Definitions */ +#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */ +#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */ + +/* DWT Folded-instruction Count Register Definitions */ +#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */ +#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */ + +/* DWT Comparator Mask Register Definitions */ +#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */ +#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */ + +/* DWT Comparator Function Register Definitions */ +#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */ +#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */ + +#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */ +#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */ + +#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */ +#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */ + +#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */ +#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */ + +#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */ +#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */ + +#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */ +#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */ + +#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */ +#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */ + +#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */ +#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */ + +#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */ +#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */ + +/*@}*/ /* end of group CMSIS_DWT */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_TPI Trace Port Interface (TPI) + \brief Type definitions for the Trace Port Interface (TPI) + @{ + */ + +/** \brief Structure type to access the Trace Port Interface Register (TPI). + */ +typedef struct +{ + __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */ + __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */ + uint32_t RESERVED0[2]; + __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */ + uint32_t RESERVED1[55]; + __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */ + uint32_t RESERVED2[131]; + __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */ + __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */ + __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */ + uint32_t RESERVED3[759]; + __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */ + __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */ + __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */ + uint32_t RESERVED4[1]; + __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */ + __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */ + __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */ + uint32_t RESERVED5[39]; + __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */ + __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */ + uint32_t RESERVED7[8]; + __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */ + __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */ +} TPI_Type; + +/* TPI Asynchronous Clock Prescaler Register Definitions */ +#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */ +#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */ + +/* TPI Selected Pin Protocol Register Definitions */ +#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */ +#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */ + +/* TPI Formatter and Flush Status Register Definitions */ +#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */ +#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */ + +#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */ +#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */ + +#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */ +#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */ + +#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */ +#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */ + +/* TPI Formatter and Flush Control Register Definitions */ +#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */ +#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */ + +#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */ +#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */ + +/* TPI TRIGGER Register Definitions */ +#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */ +#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */ + +/* TPI Integration ETM Data Register Definitions (FIFO0) */ +#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */ +#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */ + +#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */ +#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */ + +#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */ +#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */ + +#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */ +#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */ + +#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */ +#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */ + +#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */ +#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */ + +#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */ +#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */ + +/* TPI ITATBCTR2 Register Definitions */ +#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */ +#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */ + +/* TPI Integration ITM Data Register Definitions (FIFO1) */ +#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */ +#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */ + +#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */ +#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */ + +#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */ +#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */ + +#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */ +#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */ + +#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */ +#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */ + +#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */ +#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */ + +#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */ +#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */ + +/* TPI ITATBCTR0 Register Definitions */ +#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */ +#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */ + +/* TPI Integration Mode Control Register Definitions */ +#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */ +#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */ + +/* TPI DEVID Register Definitions */ +#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */ +#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */ + +#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */ +#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */ + +#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */ +#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */ + +#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */ +#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */ + +#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */ +#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */ + +#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */ +#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */ + +/* TPI DEVTYPE Register Definitions */ +#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */ +#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */ + +#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */ +#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */ + +/*@}*/ /* end of group CMSIS_TPI */ + + +#if (__MPU_PRESENT == 1) +/** \ingroup CMSIS_core_register + \defgroup CMSIS_MPU Memory Protection Unit (MPU) + \brief Type definitions for the Memory Protection Unit (MPU) + @{ + */ + +/** \brief Structure type to access the Memory Protection Unit (MPU). + */ +typedef struct +{ + __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */ + __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */ + __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */ + __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */ + __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */ + __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */ + __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */ + __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */ + __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */ + __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */ + __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */ +} MPU_Type; + +/* MPU Type Register */ +#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */ +#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */ + +#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */ +#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */ + +#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */ +#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */ + +/* MPU Control Register */ +#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */ +#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */ + +#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */ +#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */ + +#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */ +#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */ + +/* MPU Region Number Register */ +#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */ +#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */ + +/* MPU Region Base Address Register */ +#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */ +#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */ + +#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */ +#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */ + +#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */ +#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */ + +/* MPU Region Attribute and Size Register */ +#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */ +#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */ + +#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */ +#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */ + +#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */ +#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */ + +#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */ +#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */ + +#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */ +#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */ + +#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */ +#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */ + +#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */ +#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */ + +#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */ +#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */ + +#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */ +#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */ + +#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */ +#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */ + +/*@} end of group CMSIS_MPU */ +#endif + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug) + \brief Type definitions for the Core Debug Registers + @{ + */ + +/** \brief Structure type to access the Core Debug Register (CoreDebug). + */ +typedef struct +{ + __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */ + __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */ + __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */ + __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */ +} CoreDebug_Type; + +/* Debug Halting Control and Status Register */ +#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */ +#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */ + +#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */ +#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */ + +#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */ +#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */ + +#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */ +#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */ + +#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */ +#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */ + +#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */ +#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */ + +#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */ +#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */ + +#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */ +#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */ + +#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */ +#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */ + +#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */ +#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */ + +#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */ +#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */ + +#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */ +#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */ + +/* Debug Core Register Selector Register */ +#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */ +#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */ + +#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */ +#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */ + +/* Debug Exception and Monitor Control Register */ +#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */ +#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */ + +#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */ +#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */ + +#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */ +#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */ + +#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */ +#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */ + +#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */ +#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */ + +#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */ +#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */ + +#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */ +#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */ + +#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */ +#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */ + +#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */ +#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */ + +#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */ +#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */ + +#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */ +#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */ + +#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */ +#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */ + +#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */ +#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */ + +/*@} end of group CMSIS_CoreDebug */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_core_base Core Definitions + \brief Definitions for base addresses, unions, and structures. + @{ + */ + +/* Memory mapping of Cortex-M3 Hardware */ +#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */ +#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */ +#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */ +#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */ +#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */ +#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */ +#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */ +#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */ + +#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */ +#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */ +#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */ +#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */ +#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */ +#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */ +#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */ +#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */ + +#if (__MPU_PRESENT == 1) + #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */ + #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */ +#endif + +/*@} */ + + + +/******************************************************************************* + * Hardware Abstraction Layer + Core Function Interface contains: + - Core NVIC Functions + - Core SysTick Functions + - Core Debug Functions + - Core Register Access Functions + ******************************************************************************/ +/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference +*/ + + + +/* ########################## NVIC functions #################################### */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_Core_NVICFunctions NVIC Functions + \brief Functions that manage interrupts and exceptions via the NVIC. + @{ + */ + +/** \brief Set Priority Grouping + + The function sets the priority grouping field using the required unlock sequence. + The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field. + Only values from 0..7 are used. + In case of a conflict between priority grouping and available + priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set. + + \param [in] PriorityGroup Priority grouping field. + */ +__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) +{ + uint32_t reg_value; + uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */ + + reg_value = SCB->AIRCR; /* read old register configuration */ + reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */ + reg_value = (reg_value | + ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) | + (PriorityGroupTmp << 8)); /* Insert write key and priorty group */ + SCB->AIRCR = reg_value; +} + + +/** \brief Get Priority Grouping + + The function reads the priority grouping field from the NVIC Interrupt Controller. + + \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field). + */ +__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void) +{ + return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */ +} + + +/** \brief Enable External Interrupt + + The function enables a device-specific interrupt in the NVIC interrupt controller. + + \param [in] IRQn External interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn) +{ + NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */ +} + + +/** \brief Disable External Interrupt + + The function disables a device-specific interrupt in the NVIC interrupt controller. + + \param [in] IRQn External interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn) +{ + NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */ +} + + +/** \brief Get Pending Interrupt + + The function reads the pending register in the NVIC and returns the pending bit + for the specified interrupt. + + \param [in] IRQn Interrupt number. + + \return 0 Interrupt status is not pending. + \return 1 Interrupt status is pending. + */ +__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn) +{ + return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */ +} + + +/** \brief Set Pending Interrupt + + The function sets the pending bit of an external interrupt. + + \param [in] IRQn Interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn) +{ + NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */ +} + + +/** \brief Clear Pending Interrupt + + The function clears the pending bit of an external interrupt. + + \param [in] IRQn External interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn) +{ + NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */ +} + + +/** \brief Get Active Interrupt + + The function reads the active register in NVIC and returns the active bit. + + \param [in] IRQn Interrupt number. + + \return 0 Interrupt status is not active. + \return 1 Interrupt status is active. + */ +__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn) +{ + return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */ +} + + +/** \brief Set Interrupt Priority + + The function sets the priority of an interrupt. + + \note The priority cannot be set for every core interrupt. + + \param [in] IRQn Interrupt number. + \param [in] priority Priority to set. + */ +__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority) +{ + if(IRQn < 0) { + SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */ + else { + NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */ +} + + +/** \brief Get Interrupt Priority + + The function reads the priority of an interrupt. The interrupt + number can be positive to specify an external (device specific) + interrupt, or negative to specify an internal (core) interrupt. + + + \param [in] IRQn Interrupt number. + \return Interrupt Priority. Value is aligned automatically to the implemented + priority bits of the microcontroller. + */ +__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn) +{ + + if(IRQn < 0) { + return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */ + else { + return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */ +} + + +/** \brief Encode Priority + + The function encodes the priority for an interrupt with the given priority group, + preemptive priority value, and subpriority value. + In case of a conflict between priority grouping and available + priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set. + + \param [in] PriorityGroup Used priority group. + \param [in] PreemptPriority Preemptive priority value (starting from 0). + \param [in] SubPriority Subpriority value (starting from 0). + \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority(). + */ +__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority) +{ + uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ + uint32_t PreemptPriorityBits; + uint32_t SubPriorityBits; + + PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; + SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; + + return ( + ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) | + ((SubPriority & ((1 << (SubPriorityBits )) - 1))) + ); +} + + +/** \brief Decode Priority + + The function decodes an interrupt priority value with a given priority group to + preemptive priority value and subpriority value. + In case of a conflict between priority grouping and available + priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set. + + \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority(). + \param [in] PriorityGroup Used priority group. + \param [out] pPreemptPriority Preemptive priority value (starting from 0). + \param [out] pSubPriority Subpriority value (starting from 0). + */ +__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority) +{ + uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ + uint32_t PreemptPriorityBits; + uint32_t SubPriorityBits; + + PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; + SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; + + *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1); + *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1); +} + + +/** \brief System Reset + + The function initiates a system reset request to reset the MCU. + */ +__STATIC_INLINE void NVIC_SystemReset(void) +{ + __DSB(); /* Ensure all outstanding memory accesses included + buffered write are completed before reset */ + SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) | + (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | + SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */ + __DSB(); /* Ensure completion of memory access */ + while(1); /* wait until reset */ +} + +/*@} end of CMSIS_Core_NVICFunctions */ + + + +/* ################################## SysTick function ############################################ */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_Core_SysTickFunctions SysTick Functions + \brief Functions that configure the System. + @{ + */ + +#if (__Vendor_SysTickConfig == 0) + +/** \brief System Tick Configuration + + The function initializes the System Timer and its interrupt, and starts the System Tick Timer. + Counter is in free running mode to generate periodic interrupts. + + \param [in] ticks Number of ticks between two interrupts. + + \return 0 Function succeeded. + \return 1 Function failed. + + \note When the variable __Vendor_SysTickConfig is set to 1, then the + function SysTick_Config is not included. In this case, the file device.h + must contain a vendor-specific implementation of this function. + + */ +__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks) +{ + if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */ + + SysTick->LOAD = ticks - 1; /* set reload register */ + NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */ + SysTick->VAL = 0; /* Load the SysTick Counter Value */ + SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | + SysTick_CTRL_TICKINT_Msk | + SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */ + return (0); /* Function successful */ +} + +#endif + +/*@} end of CMSIS_Core_SysTickFunctions */ + + + +/* ##################################### Debug In/Output function ########################################### */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_core_DebugFunctions ITM Functions + \brief Functions that access the ITM debug interface. + @{ + */ + +extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */ +#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */ + + +/** \brief ITM Send Character + + The function transmits a character via the ITM channel 0, and + \li Just returns when no debugger is connected that has booked the output. + \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted. + + \param [in] ch Character to transmit. + + \returns Character to transmit. + */ +__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch) +{ + if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */ + (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */ + { + while (ITM->PORT[0].u32 == 0); + ITM->PORT[0].u8 = (uint8_t) ch; + } + return (ch); +} + + +/** \brief ITM Receive Character + + The function inputs a character via the external variable \ref ITM_RxBuffer. + + \return Received character. + \return -1 No character pending. + */ +__STATIC_INLINE int32_t ITM_ReceiveChar (void) { + int32_t ch = -1; /* no character available */ + + if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) { + ch = ITM_RxBuffer; + ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */ + } + + return (ch); +} + + +/** \brief ITM Check Character + + The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer. + + \return 0 No character available. + \return 1 Character available. + */ +__STATIC_INLINE int32_t ITM_CheckChar (void) { + + if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) { + return (0); /* no character available */ + } else { + return (1); /* character available */ + } +} + +/*@} end of CMSIS_core_DebugFunctions */ + +#endif /* __CORE_CM3_H_DEPENDANT */ + +#endif /* __CMSIS_GENERIC */ + +#ifdef __cplusplus +} +#endif diff --git a/src/lib/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h new file mode 100644 index 000000000..93efd3a7a --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h @@ -0,0 +1,1772 @@ +/**************************************************************************//** + * @file core_cm4.h + * @brief CMSIS Cortex-M4 Core Peripheral Access Layer Header File + * @version V3.20 + * @date 25. February 2013 + * + * @note + * + ******************************************************************************/ +/* Copyright (c) 2009 - 2013 ARM LIMITED + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - 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. + - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. + ---------------------------------------------------------------------------*/ + + +#if defined ( __ICCARM__ ) + #pragma system_include /* treat file as system include file for MISRA check */ +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +#ifndef __CORE_CM4_H_GENERIC +#define __CORE_CM4_H_GENERIC + +/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions + CMSIS violates the following MISRA-C:2004 rules: + + \li Required Rule 8.5, object/function definition in header file.
+ Function definitions in header files are used to allow 'inlining'. + + \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.
+ Unions are used for effective representation of core registers. + + \li Advisory Rule 19.7, Function-like macro defined.
+ Function-like macros are used to allow more efficient code. + */ + + +/******************************************************************************* + * CMSIS definitions + ******************************************************************************/ +/** \ingroup Cortex_M4 + @{ + */ + +/* CMSIS CM4 definitions */ +#define __CM4_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */ +#define __CM4_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */ +#define __CM4_CMSIS_VERSION ((__CM4_CMSIS_VERSION_MAIN << 16) | \ + __CM4_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */ + +#define __CORTEX_M (0x04) /*!< Cortex-M Core */ + + +#if defined ( __CC_ARM ) + #define __ASM __asm /*!< asm keyword for ARM Compiler */ + #define __INLINE __inline /*!< inline keyword for ARM Compiler */ + #define __STATIC_INLINE static __inline + +#elif defined ( __ICCARM__ ) + #define __ASM __asm /*!< asm keyword for IAR Compiler */ + #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */ + #define __STATIC_INLINE static inline + +#elif defined ( __TMS470__ ) + #define __ASM __asm /*!< asm keyword for TI CCS Compiler */ + #define __STATIC_INLINE static inline + +#elif defined ( __GNUC__ ) + #define __ASM __asm /*!< asm keyword for GNU Compiler */ + #define __INLINE inline /*!< inline keyword for GNU Compiler */ + #define __STATIC_INLINE static inline + +#elif defined ( __TASKING__ ) + #define __ASM __asm /*!< asm keyword for TASKING Compiler */ + #define __INLINE inline /*!< inline keyword for TASKING Compiler */ + #define __STATIC_INLINE static inline + +#endif + +/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions. +*/ +#if defined ( __CC_ARM ) + #if defined __TARGET_FPU_VFP + #if (__FPU_PRESENT == 1) + #define __FPU_USED 1 + #else + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #define __FPU_USED 0 + #endif + #else + #define __FPU_USED 0 + #endif + +#elif defined ( __ICCARM__ ) + #if defined __ARMVFP__ + #if (__FPU_PRESENT == 1) + #define __FPU_USED 1 + #else + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #define __FPU_USED 0 + #endif + #else + #define __FPU_USED 0 + #endif + +#elif defined ( __TMS470__ ) + #if defined __TI_VFP_SUPPORT__ + #if (__FPU_PRESENT == 1) + #define __FPU_USED 1 + #else + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #define __FPU_USED 0 + #endif + #else + #define __FPU_USED 0 + #endif + +#elif defined ( __GNUC__ ) + #if defined (__VFP_FP__) && !defined(__SOFTFP__) + #if (__FPU_PRESENT == 1) + #define __FPU_USED 1 + #else + #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #define __FPU_USED 0 + #endif + #else + #define __FPU_USED 0 + #endif + +#elif defined ( __TASKING__ ) + #if defined __FPU_VFP__ + #if (__FPU_PRESENT == 1) + #define __FPU_USED 1 + #else + #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" + #define __FPU_USED 0 + #endif + #else + #define __FPU_USED 0 + #endif +#endif + +#include /* standard types definitions */ +#include "core_cmInstr.h" /* Core Instruction Access */ +#include "core_cmFunc.h" /* Core Function Access */ +#include "core_cm4_simd.h" /* Compiler specific SIMD Intrinsics */ + +#endif /* __CORE_CM4_H_GENERIC */ + +#ifndef __CMSIS_GENERIC + +#ifndef __CORE_CM4_H_DEPENDANT +#define __CORE_CM4_H_DEPENDANT + +/* check device defines and use defaults */ +#if defined __CHECK_DEVICE_DEFINES + #ifndef __CM4_REV + #define __CM4_REV 0x0000 + #warning "__CM4_REV not defined in device header file; using default!" + #endif + + #ifndef __FPU_PRESENT + #define __FPU_PRESENT 0 + #warning "__FPU_PRESENT not defined in device header file; using default!" + #endif + + #ifndef __MPU_PRESENT + #define __MPU_PRESENT 0 + #warning "__MPU_PRESENT not defined in device header file; using default!" + #endif + + #ifndef __NVIC_PRIO_BITS + #define __NVIC_PRIO_BITS 4 + #warning "__NVIC_PRIO_BITS not defined in device header file; using default!" + #endif + + #ifndef __Vendor_SysTickConfig + #define __Vendor_SysTickConfig 0 + #warning "__Vendor_SysTickConfig not defined in device header file; using default!" + #endif +#endif + +/* IO definitions (access restrictions to peripheral registers) */ +/** + \defgroup CMSIS_glob_defs CMSIS Global Defines + + IO Type Qualifiers are used + \li to specify the access to peripheral variables. + \li for automatic generation of peripheral register debug information. +*/ +#ifdef __cplusplus + #define __I volatile /*!< Defines 'read only' permissions */ +#else + #define __I volatile const /*!< Defines 'read only' permissions */ +#endif +#define __O volatile /*!< Defines 'write only' permissions */ +#define __IO volatile /*!< Defines 'read / write' permissions */ + +/*@} end of group Cortex_M4 */ + + + +/******************************************************************************* + * Register Abstraction + Core Register contain: + - Core Register + - Core NVIC Register + - Core SCB Register + - Core SysTick Register + - Core Debug Register + - Core MPU Register + - Core FPU Register + ******************************************************************************/ +/** \defgroup CMSIS_core_register Defines and Type Definitions + \brief Type definitions and defines for Cortex-M processor based devices. +*/ + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_CORE Status and Control Registers + \brief Core Register type definitions. + @{ + */ + +/** \brief Union type to access the Application Program Status Register (APSR). + */ +typedef union +{ + struct + { +#if (__CORTEX_M != 0x04) + uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */ +#else + uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */ + uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ + uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */ +#endif + uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ + uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ + uint32_t C:1; /*!< bit: 29 Carry condition code flag */ + uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ + uint32_t N:1; /*!< bit: 31 Negative condition code flag */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} APSR_Type; + + +/** \brief Union type to access the Interrupt Program Status Register (IPSR). + */ +typedef union +{ + struct + { + uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ + uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} IPSR_Type; + + +/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR). + */ +typedef union +{ + struct + { + uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ +#if (__CORTEX_M != 0x04) + uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */ +#else + uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */ + uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ + uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */ +#endif + uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */ + uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */ + uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ + uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ + uint32_t C:1; /*!< bit: 29 Carry condition code flag */ + uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ + uint32_t N:1; /*!< bit: 31 Negative condition code flag */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} xPSR_Type; + + +/** \brief Union type to access the Control Registers (CONTROL). + */ +typedef union +{ + struct + { + uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */ + uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */ + uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */ + uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */ + } b; /*!< Structure used for bit access */ + uint32_t w; /*!< Type used for word access */ +} CONTROL_Type; + +/*@} end of group CMSIS_CORE */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC) + \brief Type definitions for the NVIC Registers + @{ + */ + +/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC). + */ +typedef struct +{ + __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ + uint32_t RESERVED0[24]; + __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ + uint32_t RSERVED1[24]; + __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ + uint32_t RESERVED2[24]; + __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ + uint32_t RESERVED3[24]; + __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ + uint32_t RESERVED4[56]; + __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ + uint32_t RESERVED5[644]; + __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ +} NVIC_Type; + +/* Software Triggered Interrupt Register Definitions */ +#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */ +#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */ + +/*@} end of group CMSIS_NVIC */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_SCB System Control Block (SCB) + \brief Type definitions for the System Control Block Registers + @{ + */ + +/** \brief Structure type to access the System Control Block (SCB). + */ +typedef struct +{ + __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */ + __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */ + __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */ + __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */ + __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */ + __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */ + __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */ + __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */ + __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */ + __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */ + __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */ + __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */ + __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */ + __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */ + __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */ + __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */ + __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */ + __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */ + __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */ + uint32_t RESERVED0[5]; + __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */ +} SCB_Type; + +/* SCB CPUID Register Definitions */ +#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */ +#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */ + +#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */ +#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */ + +#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */ +#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */ + +#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */ +#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */ + +#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */ +#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */ + +/* SCB Interrupt Control State Register Definitions */ +#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */ +#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */ + +#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */ +#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */ + +#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */ +#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */ + +#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */ +#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */ + +#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */ +#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */ + +#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */ +#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */ + +#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */ +#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */ + +#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */ +#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */ + +#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */ +#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */ + +#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */ +#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */ + +/* SCB Vector Table Offset Register Definitions */ +#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */ +#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ + +/* SCB Application Interrupt and Reset Control Register Definitions */ +#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */ +#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */ + +#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */ +#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */ + +#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */ +#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */ + +#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */ +#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ + +#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */ +#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */ + +#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */ +#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */ + +#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */ +#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */ + +/* SCB System Control Register Definitions */ +#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */ +#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */ + +#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */ +#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */ + +#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */ +#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */ + +/* SCB Configuration Control Register Definitions */ +#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */ +#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */ + +#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */ +#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */ + +#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */ +#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */ + +#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */ +#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */ + +#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */ +#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */ + +#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */ +#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */ + +/* SCB System Handler Control and State Register Definitions */ +#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */ +#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */ + +#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */ +#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */ + +#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */ +#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */ + +#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */ +#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */ + +#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */ +#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */ + +#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */ +#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */ + +#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */ +#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */ + +#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */ +#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */ + +#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */ +#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */ + +#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */ +#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */ + +#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */ +#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */ + +#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */ +#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */ + +#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */ +#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */ + +#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */ +#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */ + +/* SCB Configurable Fault Status Registers Definitions */ +#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */ +#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */ + +#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */ +#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */ + +#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */ +#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */ + +/* SCB Hard Fault Status Registers Definitions */ +#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */ +#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */ + +#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */ +#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */ + +#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */ +#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */ + +/* SCB Debug Fault Status Register Definitions */ +#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */ +#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */ + +#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */ +#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */ + +#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */ +#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */ + +#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */ +#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */ + +#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */ +#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */ + +/*@} end of group CMSIS_SCB */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB) + \brief Type definitions for the System Control and ID Register not in the SCB + @{ + */ + +/** \brief Structure type to access the System Control and ID Register not in the SCB. + */ +typedef struct +{ + uint32_t RESERVED0[1]; + __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */ + __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */ +} SCnSCB_Type; + +/* Interrupt Controller Type Register Definitions */ +#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */ +#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */ + +/* Auxiliary Control Register Definitions */ +#define SCnSCB_ACTLR_DISOOFP_Pos 9 /*!< ACTLR: DISOOFP Position */ +#define SCnSCB_ACTLR_DISOOFP_Msk (1UL << SCnSCB_ACTLR_DISOOFP_Pos) /*!< ACTLR: DISOOFP Mask */ + +#define SCnSCB_ACTLR_DISFPCA_Pos 8 /*!< ACTLR: DISFPCA Position */ +#define SCnSCB_ACTLR_DISFPCA_Msk (1UL << SCnSCB_ACTLR_DISFPCA_Pos) /*!< ACTLR: DISFPCA Mask */ + +#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */ +#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */ + +#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */ +#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */ + +#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */ +#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */ + +/*@} end of group CMSIS_SCnotSCB */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_SysTick System Tick Timer (SysTick) + \brief Type definitions for the System Timer Registers. + @{ + */ + +/** \brief Structure type to access the System Timer (SysTick). + */ +typedef struct +{ + __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */ + __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */ + __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */ + __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */ +} SysTick_Type; + +/* SysTick Control / Status Register Definitions */ +#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */ +#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */ + +#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */ +#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */ + +#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */ +#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */ + +#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */ +#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */ + +/* SysTick Reload Register Definitions */ +#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */ +#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */ + +/* SysTick Current Register Definitions */ +#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */ +#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */ + +/* SysTick Calibration Register Definitions */ +#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */ +#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */ + +#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */ +#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */ + +#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */ +#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */ + +/*@} end of group CMSIS_SysTick */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM) + \brief Type definitions for the Instrumentation Trace Macrocell (ITM) + @{ + */ + +/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM). + */ +typedef struct +{ + __O union + { + __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */ + __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */ + __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */ + } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */ + uint32_t RESERVED0[864]; + __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */ + uint32_t RESERVED1[15]; + __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */ + uint32_t RESERVED2[15]; + __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */ + uint32_t RESERVED3[29]; + __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */ + __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */ + __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */ + uint32_t RESERVED4[43]; + __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */ + __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */ + uint32_t RESERVED5[6]; + __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */ + __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */ + __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */ + __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */ + __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */ + __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */ + __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */ + __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */ + __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */ + __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */ + __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */ + __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */ +} ITM_Type; + +/* ITM Trace Privilege Register Definitions */ +#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */ +#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */ + +/* ITM Trace Control Register Definitions */ +#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */ +#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */ + +#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */ +#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */ + +#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */ +#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */ + +#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */ +#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */ + +#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */ +#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */ + +#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */ +#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */ + +#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */ +#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */ + +#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */ +#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */ + +#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */ +#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */ + +/* ITM Integration Write Register Definitions */ +#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */ +#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */ + +/* ITM Integration Read Register Definitions */ +#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */ +#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */ + +/* ITM Integration Mode Control Register Definitions */ +#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */ +#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */ + +/* ITM Lock Status Register Definitions */ +#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */ +#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */ + +#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */ +#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */ + +#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */ +#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */ + +/*@}*/ /* end of group CMSIS_ITM */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT) + \brief Type definitions for the Data Watchpoint and Trace (DWT) + @{ + */ + +/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT). + */ +typedef struct +{ + __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */ + __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */ + __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */ + __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */ + __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */ + __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */ + __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */ + __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */ + __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */ + __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */ + __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */ + uint32_t RESERVED0[1]; + __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */ + __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */ + __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */ + uint32_t RESERVED1[1]; + __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */ + __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */ + __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */ + uint32_t RESERVED2[1]; + __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */ + __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */ + __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */ +} DWT_Type; + +/* DWT Control Register Definitions */ +#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */ +#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */ + +#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */ +#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */ + +#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */ +#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */ + +#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */ +#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */ + +#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */ +#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */ + +#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */ +#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */ + +#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */ +#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */ + +#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */ +#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */ + +#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */ +#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */ + +#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */ +#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */ + +#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */ +#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */ + +#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */ +#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */ + +#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */ +#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */ + +#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */ +#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */ + +#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */ +#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */ + +#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */ +#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */ + +#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */ +#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */ + +#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */ +#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */ + +/* DWT CPI Count Register Definitions */ +#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */ +#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */ + +/* DWT Exception Overhead Count Register Definitions */ +#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */ +#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */ + +/* DWT Sleep Count Register Definitions */ +#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */ +#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */ + +/* DWT LSU Count Register Definitions */ +#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */ +#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */ + +/* DWT Folded-instruction Count Register Definitions */ +#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */ +#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */ + +/* DWT Comparator Mask Register Definitions */ +#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */ +#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */ + +/* DWT Comparator Function Register Definitions */ +#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */ +#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */ + +#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */ +#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */ + +#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */ +#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */ + +#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */ +#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */ + +#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */ +#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */ + +#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */ +#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */ + +#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */ +#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */ + +#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */ +#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */ + +#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */ +#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */ + +/*@}*/ /* end of group CMSIS_DWT */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_TPI Trace Port Interface (TPI) + \brief Type definitions for the Trace Port Interface (TPI) + @{ + */ + +/** \brief Structure type to access the Trace Port Interface Register (TPI). + */ +typedef struct +{ + __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */ + __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */ + uint32_t RESERVED0[2]; + __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */ + uint32_t RESERVED1[55]; + __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */ + uint32_t RESERVED2[131]; + __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */ + __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */ + __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */ + uint32_t RESERVED3[759]; + __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */ + __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */ + __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */ + uint32_t RESERVED4[1]; + __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */ + __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */ + __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */ + uint32_t RESERVED5[39]; + __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */ + __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */ + uint32_t RESERVED7[8]; + __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */ + __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */ +} TPI_Type; + +/* TPI Asynchronous Clock Prescaler Register Definitions */ +#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */ +#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */ + +/* TPI Selected Pin Protocol Register Definitions */ +#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */ +#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */ + +/* TPI Formatter and Flush Status Register Definitions */ +#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */ +#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */ + +#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */ +#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */ + +#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */ +#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */ + +#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */ +#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */ + +/* TPI Formatter and Flush Control Register Definitions */ +#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */ +#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */ + +#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */ +#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */ + +/* TPI TRIGGER Register Definitions */ +#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */ +#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */ + +/* TPI Integration ETM Data Register Definitions (FIFO0) */ +#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */ +#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */ + +#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */ +#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */ + +#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */ +#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */ + +#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */ +#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */ + +#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */ +#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */ + +#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */ +#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */ + +#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */ +#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */ + +/* TPI ITATBCTR2 Register Definitions */ +#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */ +#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */ + +/* TPI Integration ITM Data Register Definitions (FIFO1) */ +#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */ +#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */ + +#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */ +#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */ + +#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */ +#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */ + +#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */ +#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */ + +#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */ +#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */ + +#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */ +#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */ + +#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */ +#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */ + +/* TPI ITATBCTR0 Register Definitions */ +#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */ +#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */ + +/* TPI Integration Mode Control Register Definitions */ +#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */ +#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */ + +/* TPI DEVID Register Definitions */ +#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */ +#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */ + +#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */ +#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */ + +#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */ +#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */ + +#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */ +#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */ + +#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */ +#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */ + +#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */ +#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */ + +/* TPI DEVTYPE Register Definitions */ +#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */ +#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */ + +#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */ +#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */ + +/*@}*/ /* end of group CMSIS_TPI */ + + +#if (__MPU_PRESENT == 1) +/** \ingroup CMSIS_core_register + \defgroup CMSIS_MPU Memory Protection Unit (MPU) + \brief Type definitions for the Memory Protection Unit (MPU) + @{ + */ + +/** \brief Structure type to access the Memory Protection Unit (MPU). + */ +typedef struct +{ + __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */ + __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */ + __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */ + __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */ + __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */ + __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */ + __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */ + __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */ + __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */ + __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */ + __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */ +} MPU_Type; + +/* MPU Type Register */ +#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */ +#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */ + +#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */ +#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */ + +#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */ +#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */ + +/* MPU Control Register */ +#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */ +#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */ + +#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */ +#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */ + +#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */ +#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */ + +/* MPU Region Number Register */ +#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */ +#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */ + +/* MPU Region Base Address Register */ +#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */ +#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */ + +#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */ +#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */ + +#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */ +#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */ + +/* MPU Region Attribute and Size Register */ +#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */ +#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */ + +#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */ +#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */ + +#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */ +#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */ + +#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */ +#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */ + +#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */ +#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */ + +#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */ +#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */ + +#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */ +#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */ + +#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */ +#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */ + +#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */ +#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */ + +#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */ +#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */ + +/*@} end of group CMSIS_MPU */ +#endif + + +#if (__FPU_PRESENT == 1) +/** \ingroup CMSIS_core_register + \defgroup CMSIS_FPU Floating Point Unit (FPU) + \brief Type definitions for the Floating Point Unit (FPU) + @{ + */ + +/** \brief Structure type to access the Floating Point Unit (FPU). + */ +typedef struct +{ + uint32_t RESERVED0[1]; + __IO uint32_t FPCCR; /*!< Offset: 0x004 (R/W) Floating-Point Context Control Register */ + __IO uint32_t FPCAR; /*!< Offset: 0x008 (R/W) Floating-Point Context Address Register */ + __IO uint32_t FPDSCR; /*!< Offset: 0x00C (R/W) Floating-Point Default Status Control Register */ + __I uint32_t MVFR0; /*!< Offset: 0x010 (R/ ) Media and FP Feature Register 0 */ + __I uint32_t MVFR1; /*!< Offset: 0x014 (R/ ) Media and FP Feature Register 1 */ +} FPU_Type; + +/* Floating-Point Context Control Register */ +#define FPU_FPCCR_ASPEN_Pos 31 /*!< FPCCR: ASPEN bit Position */ +#define FPU_FPCCR_ASPEN_Msk (1UL << FPU_FPCCR_ASPEN_Pos) /*!< FPCCR: ASPEN bit Mask */ + +#define FPU_FPCCR_LSPEN_Pos 30 /*!< FPCCR: LSPEN Position */ +#define FPU_FPCCR_LSPEN_Msk (1UL << FPU_FPCCR_LSPEN_Pos) /*!< FPCCR: LSPEN bit Mask */ + +#define FPU_FPCCR_MONRDY_Pos 8 /*!< FPCCR: MONRDY Position */ +#define FPU_FPCCR_MONRDY_Msk (1UL << FPU_FPCCR_MONRDY_Pos) /*!< FPCCR: MONRDY bit Mask */ + +#define FPU_FPCCR_BFRDY_Pos 6 /*!< FPCCR: BFRDY Position */ +#define FPU_FPCCR_BFRDY_Msk (1UL << FPU_FPCCR_BFRDY_Pos) /*!< FPCCR: BFRDY bit Mask */ + +#define FPU_FPCCR_MMRDY_Pos 5 /*!< FPCCR: MMRDY Position */ +#define FPU_FPCCR_MMRDY_Msk (1UL << FPU_FPCCR_MMRDY_Pos) /*!< FPCCR: MMRDY bit Mask */ + +#define FPU_FPCCR_HFRDY_Pos 4 /*!< FPCCR: HFRDY Position */ +#define FPU_FPCCR_HFRDY_Msk (1UL << FPU_FPCCR_HFRDY_Pos) /*!< FPCCR: HFRDY bit Mask */ + +#define FPU_FPCCR_THREAD_Pos 3 /*!< FPCCR: processor mode bit Position */ +#define FPU_FPCCR_THREAD_Msk (1UL << FPU_FPCCR_THREAD_Pos) /*!< FPCCR: processor mode active bit Mask */ + +#define FPU_FPCCR_USER_Pos 1 /*!< FPCCR: privilege level bit Position */ +#define FPU_FPCCR_USER_Msk (1UL << FPU_FPCCR_USER_Pos) /*!< FPCCR: privilege level bit Mask */ + +#define FPU_FPCCR_LSPACT_Pos 0 /*!< FPCCR: Lazy state preservation active bit Position */ +#define FPU_FPCCR_LSPACT_Msk (1UL << FPU_FPCCR_LSPACT_Pos) /*!< FPCCR: Lazy state preservation active bit Mask */ + +/* Floating-Point Context Address Register */ +#define FPU_FPCAR_ADDRESS_Pos 3 /*!< FPCAR: ADDRESS bit Position */ +#define FPU_FPCAR_ADDRESS_Msk (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos) /*!< FPCAR: ADDRESS bit Mask */ + +/* Floating-Point Default Status Control Register */ +#define FPU_FPDSCR_AHP_Pos 26 /*!< FPDSCR: AHP bit Position */ +#define FPU_FPDSCR_AHP_Msk (1UL << FPU_FPDSCR_AHP_Pos) /*!< FPDSCR: AHP bit Mask */ + +#define FPU_FPDSCR_DN_Pos 25 /*!< FPDSCR: DN bit Position */ +#define FPU_FPDSCR_DN_Msk (1UL << FPU_FPDSCR_DN_Pos) /*!< FPDSCR: DN bit Mask */ + +#define FPU_FPDSCR_FZ_Pos 24 /*!< FPDSCR: FZ bit Position */ +#define FPU_FPDSCR_FZ_Msk (1UL << FPU_FPDSCR_FZ_Pos) /*!< FPDSCR: FZ bit Mask */ + +#define FPU_FPDSCR_RMode_Pos 22 /*!< FPDSCR: RMode bit Position */ +#define FPU_FPDSCR_RMode_Msk (3UL << FPU_FPDSCR_RMode_Pos) /*!< FPDSCR: RMode bit Mask */ + +/* Media and FP Feature Register 0 */ +#define FPU_MVFR0_FP_rounding_modes_Pos 28 /*!< MVFR0: FP rounding modes bits Position */ +#define FPU_MVFR0_FP_rounding_modes_Msk (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos) /*!< MVFR0: FP rounding modes bits Mask */ + +#define FPU_MVFR0_Short_vectors_Pos 24 /*!< MVFR0: Short vectors bits Position */ +#define FPU_MVFR0_Short_vectors_Msk (0xFUL << FPU_MVFR0_Short_vectors_Pos) /*!< MVFR0: Short vectors bits Mask */ + +#define FPU_MVFR0_Square_root_Pos 20 /*!< MVFR0: Square root bits Position */ +#define FPU_MVFR0_Square_root_Msk (0xFUL << FPU_MVFR0_Square_root_Pos) /*!< MVFR0: Square root bits Mask */ + +#define FPU_MVFR0_Divide_Pos 16 /*!< MVFR0: Divide bits Position */ +#define FPU_MVFR0_Divide_Msk (0xFUL << FPU_MVFR0_Divide_Pos) /*!< MVFR0: Divide bits Mask */ + +#define FPU_MVFR0_FP_excep_trapping_Pos 12 /*!< MVFR0: FP exception trapping bits Position */ +#define FPU_MVFR0_FP_excep_trapping_Msk (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos) /*!< MVFR0: FP exception trapping bits Mask */ + +#define FPU_MVFR0_Double_precision_Pos 8 /*!< MVFR0: Double-precision bits Position */ +#define FPU_MVFR0_Double_precision_Msk (0xFUL << FPU_MVFR0_Double_precision_Pos) /*!< MVFR0: Double-precision bits Mask */ + +#define FPU_MVFR0_Single_precision_Pos 4 /*!< MVFR0: Single-precision bits Position */ +#define FPU_MVFR0_Single_precision_Msk (0xFUL << FPU_MVFR0_Single_precision_Pos) /*!< MVFR0: Single-precision bits Mask */ + +#define FPU_MVFR0_A_SIMD_registers_Pos 0 /*!< MVFR0: A_SIMD registers bits Position */ +#define FPU_MVFR0_A_SIMD_registers_Msk (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos) /*!< MVFR0: A_SIMD registers bits Mask */ + +/* Media and FP Feature Register 1 */ +#define FPU_MVFR1_FP_fused_MAC_Pos 28 /*!< MVFR1: FP fused MAC bits Position */ +#define FPU_MVFR1_FP_fused_MAC_Msk (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos) /*!< MVFR1: FP fused MAC bits Mask */ + +#define FPU_MVFR1_FP_HPFP_Pos 24 /*!< MVFR1: FP HPFP bits Position */ +#define FPU_MVFR1_FP_HPFP_Msk (0xFUL << FPU_MVFR1_FP_HPFP_Pos) /*!< MVFR1: FP HPFP bits Mask */ + +#define FPU_MVFR1_D_NaN_mode_Pos 4 /*!< MVFR1: D_NaN mode bits Position */ +#define FPU_MVFR1_D_NaN_mode_Msk (0xFUL << FPU_MVFR1_D_NaN_mode_Pos) /*!< MVFR1: D_NaN mode bits Mask */ + +#define FPU_MVFR1_FtZ_mode_Pos 0 /*!< MVFR1: FtZ mode bits Position */ +#define FPU_MVFR1_FtZ_mode_Msk (0xFUL << FPU_MVFR1_FtZ_mode_Pos) /*!< MVFR1: FtZ mode bits Mask */ + +/*@} end of group CMSIS_FPU */ +#endif + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug) + \brief Type definitions for the Core Debug Registers + @{ + */ + +/** \brief Structure type to access the Core Debug Register (CoreDebug). + */ +typedef struct +{ + __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */ + __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */ + __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */ + __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */ +} CoreDebug_Type; + +/* Debug Halting Control and Status Register */ +#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */ +#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */ + +#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */ +#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */ + +#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */ +#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */ + +#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */ +#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */ + +#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */ +#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */ + +#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */ +#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */ + +#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */ +#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */ + +#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */ +#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */ + +#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */ +#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */ + +#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */ +#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */ + +#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */ +#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */ + +#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */ +#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */ + +/* Debug Core Register Selector Register */ +#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */ +#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */ + +#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */ +#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */ + +/* Debug Exception and Monitor Control Register */ +#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */ +#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */ + +#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */ +#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */ + +#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */ +#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */ + +#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */ +#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */ + +#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */ +#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */ + +#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */ +#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */ + +#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */ +#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */ + +#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */ +#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */ + +#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */ +#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */ + +#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */ +#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */ + +#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */ +#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */ + +#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */ +#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */ + +#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */ +#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */ + +/*@} end of group CMSIS_CoreDebug */ + + +/** \ingroup CMSIS_core_register + \defgroup CMSIS_core_base Core Definitions + \brief Definitions for base addresses, unions, and structures. + @{ + */ + +/* Memory mapping of Cortex-M4 Hardware */ +#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */ +#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */ +#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */ +#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */ +#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */ +#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */ +#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */ +#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */ + +#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */ +#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */ +#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */ +#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */ +#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */ +#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */ +#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */ +#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */ + +#if (__MPU_PRESENT == 1) + #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */ + #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */ +#endif + +#if (__FPU_PRESENT == 1) + #define FPU_BASE (SCS_BASE + 0x0F30UL) /*!< Floating Point Unit */ + #define FPU ((FPU_Type *) FPU_BASE ) /*!< Floating Point Unit */ +#endif + +/*@} */ + + + +/******************************************************************************* + * Hardware Abstraction Layer + Core Function Interface contains: + - Core NVIC Functions + - Core SysTick Functions + - Core Debug Functions + - Core Register Access Functions + ******************************************************************************/ +/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference +*/ + + + +/* ########################## NVIC functions #################################### */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_Core_NVICFunctions NVIC Functions + \brief Functions that manage interrupts and exceptions via the NVIC. + @{ + */ + +/** \brief Set Priority Grouping + + The function sets the priority grouping field using the required unlock sequence. + The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field. + Only values from 0..7 are used. + In case of a conflict between priority grouping and available + priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set. + + \param [in] PriorityGroup Priority grouping field. + */ +__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) +{ + uint32_t reg_value; + uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */ + + reg_value = SCB->AIRCR; /* read old register configuration */ + reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */ + reg_value = (reg_value | + ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) | + (PriorityGroupTmp << 8)); /* Insert write key and priorty group */ + SCB->AIRCR = reg_value; +} + + +/** \brief Get Priority Grouping + + The function reads the priority grouping field from the NVIC Interrupt Controller. + + \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field). + */ +__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void) +{ + return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */ +} + + +/** \brief Enable External Interrupt + + The function enables a device-specific interrupt in the NVIC interrupt controller. + + \param [in] IRQn External interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn) +{ +/* NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); enable interrupt */ + NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */ +} + + +/** \brief Disable External Interrupt + + The function disables a device-specific interrupt in the NVIC interrupt controller. + + \param [in] IRQn External interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn) +{ + NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */ +} + + +/** \brief Get Pending Interrupt + + The function reads the pending register in the NVIC and returns the pending bit + for the specified interrupt. + + \param [in] IRQn Interrupt number. + + \return 0 Interrupt status is not pending. + \return 1 Interrupt status is pending. + */ +__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn) +{ + return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */ +} + + +/** \brief Set Pending Interrupt + + The function sets the pending bit of an external interrupt. + + \param [in] IRQn Interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn) +{ + NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */ +} + + +/** \brief Clear Pending Interrupt + + The function clears the pending bit of an external interrupt. + + \param [in] IRQn External interrupt number. Value cannot be negative. + */ +__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn) +{ + NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */ +} + + +/** \brief Get Active Interrupt + + The function reads the active register in NVIC and returns the active bit. + + \param [in] IRQn Interrupt number. + + \return 0 Interrupt status is not active. + \return 1 Interrupt status is active. + */ +__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn) +{ + return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */ +} + + +/** \brief Set Interrupt Priority + + The function sets the priority of an interrupt. + + \note The priority cannot be set for every core interrupt. + + \param [in] IRQn Interrupt number. + \param [in] priority Priority to set. + */ +__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority) +{ + if(IRQn < 0) { + SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */ + else { + NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */ +} + + +/** \brief Get Interrupt Priority + + The function reads the priority of an interrupt. The interrupt + number can be positive to specify an external (device specific) + interrupt, or negative to specify an internal (core) interrupt. + + + \param [in] IRQn Interrupt number. + \return Interrupt Priority. Value is aligned automatically to the implemented + priority bits of the microcontroller. + */ +__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn) +{ + + if(IRQn < 0) { + return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */ + else { + return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */ +} + + +/** \brief Encode Priority + + The function encodes the priority for an interrupt with the given priority group, + preemptive priority value, and subpriority value. + In case of a conflict between priority grouping and available + priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set. + + \param [in] PriorityGroup Used priority group. + \param [in] PreemptPriority Preemptive priority value (starting from 0). + \param [in] SubPriority Subpriority value (starting from 0). + \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority(). + */ +__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority) +{ + uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ + uint32_t PreemptPriorityBits; + uint32_t SubPriorityBits; + + PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; + SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; + + return ( + ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) | + ((SubPriority & ((1 << (SubPriorityBits )) - 1))) + ); +} + + +/** \brief Decode Priority + + The function decodes an interrupt priority value with a given priority group to + preemptive priority value and subpriority value. + In case of a conflict between priority grouping and available + priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set. + + \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority(). + \param [in] PriorityGroup Used priority group. + \param [out] pPreemptPriority Preemptive priority value (starting from 0). + \param [out] pSubPriority Subpriority value (starting from 0). + */ +__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority) +{ + uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ + uint32_t PreemptPriorityBits; + uint32_t SubPriorityBits; + + PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; + SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; + + *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1); + *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1); +} + + +/** \brief System Reset + + The function initiates a system reset request to reset the MCU. + */ +__STATIC_INLINE void NVIC_SystemReset(void) +{ + __DSB(); /* Ensure all outstanding memory accesses included + buffered write are completed before reset */ + SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) | + (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | + SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */ + __DSB(); /* Ensure completion of memory access */ + while(1); /* wait until reset */ +} + +/*@} end of CMSIS_Core_NVICFunctions */ + + + +/* ################################## SysTick function ############################################ */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_Core_SysTickFunctions SysTick Functions + \brief Functions that configure the System. + @{ + */ + +#if (__Vendor_SysTickConfig == 0) + +/** \brief System Tick Configuration + + The function initializes the System Timer and its interrupt, and starts the System Tick Timer. + Counter is in free running mode to generate periodic interrupts. + + \param [in] ticks Number of ticks between two interrupts. + + \return 0 Function succeeded. + \return 1 Function failed. + + \note When the variable __Vendor_SysTickConfig is set to 1, then the + function SysTick_Config is not included. In this case, the file device.h + must contain a vendor-specific implementation of this function. + + */ +__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks) +{ + if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */ + + SysTick->LOAD = ticks - 1; /* set reload register */ + NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */ + SysTick->VAL = 0; /* Load the SysTick Counter Value */ + SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | + SysTick_CTRL_TICKINT_Msk | + SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */ + return (0); /* Function successful */ +} + +#endif + +/*@} end of CMSIS_Core_SysTickFunctions */ + + + +/* ##################################### Debug In/Output function ########################################### */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_core_DebugFunctions ITM Functions + \brief Functions that access the ITM debug interface. + @{ + */ + +extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */ +#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */ + + +/** \brief ITM Send Character + + The function transmits a character via the ITM channel 0, and + \li Just returns when no debugger is connected that has booked the output. + \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted. + + \param [in] ch Character to transmit. + + \returns Character to transmit. + */ +__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch) +{ + if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */ + (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */ + { + while (ITM->PORT[0].u32 == 0); + ITM->PORT[0].u8 = (uint8_t) ch; + } + return (ch); +} + + +/** \brief ITM Receive Character + + The function inputs a character via the external variable \ref ITM_RxBuffer. + + \return Received character. + \return -1 No character pending. + */ +__STATIC_INLINE int32_t ITM_ReceiveChar (void) { + int32_t ch = -1; /* no character available */ + + if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) { + ch = ITM_RxBuffer; + ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */ + } + + return (ch); +} + + +/** \brief ITM Check Character + + The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer. + + \return 0 No character available. + \return 1 Character available. + */ +__STATIC_INLINE int32_t ITM_CheckChar (void) { + + if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) { + return (0); /* no character available */ + } else { + return (1); /* character available */ + } +} + +/*@} end of CMSIS_core_DebugFunctions */ + +#endif /* __CORE_CM4_H_DEPENDANT */ + +#endif /* __CMSIS_GENERIC */ + +#ifdef __cplusplus +} +#endif diff --git a/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h new file mode 100644 index 000000000..af1831ee1 --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h @@ -0,0 +1,673 @@ +/**************************************************************************//** + * @file core_cm4_simd.h + * @brief CMSIS Cortex-M4 SIMD Header File + * @version V3.20 + * @date 25. February 2013 + * + * @note + * + ******************************************************************************/ +/* Copyright (c) 2009 - 2013 ARM LIMITED + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - 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. + - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. + ---------------------------------------------------------------------------*/ + + +#ifdef __cplusplus + extern "C" { +#endif + +#ifndef __CORE_CM4_SIMD_H +#define __CORE_CM4_SIMD_H + + +/******************************************************************************* + * Hardware Abstraction Layer + ******************************************************************************/ + + +/* ################### Compiler specific Intrinsics ########################### */ +/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics + Access to dedicated SIMD instructions + @{ +*/ + +#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ +/* ARM armcc specific functions */ + +/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ +#define __SADD8 __sadd8 +#define __QADD8 __qadd8 +#define __SHADD8 __shadd8 +#define __UADD8 __uadd8 +#define __UQADD8 __uqadd8 +#define __UHADD8 __uhadd8 +#define __SSUB8 __ssub8 +#define __QSUB8 __qsub8 +#define __SHSUB8 __shsub8 +#define __USUB8 __usub8 +#define __UQSUB8 __uqsub8 +#define __UHSUB8 __uhsub8 +#define __SADD16 __sadd16 +#define __QADD16 __qadd16 +#define __SHADD16 __shadd16 +#define __UADD16 __uadd16 +#define __UQADD16 __uqadd16 +#define __UHADD16 __uhadd16 +#define __SSUB16 __ssub16 +#define __QSUB16 __qsub16 +#define __SHSUB16 __shsub16 +#define __USUB16 __usub16 +#define __UQSUB16 __uqsub16 +#define __UHSUB16 __uhsub16 +#define __SASX __sasx +#define __QASX __qasx +#define __SHASX __shasx +#define __UASX __uasx +#define __UQASX __uqasx +#define __UHASX __uhasx +#define __SSAX __ssax +#define __QSAX __qsax +#define __SHSAX __shsax +#define __USAX __usax +#define __UQSAX __uqsax +#define __UHSAX __uhsax +#define __USAD8 __usad8 +#define __USADA8 __usada8 +#define __SSAT16 __ssat16 +#define __USAT16 __usat16 +#define __UXTB16 __uxtb16 +#define __UXTAB16 __uxtab16 +#define __SXTB16 __sxtb16 +#define __SXTAB16 __sxtab16 +#define __SMUAD __smuad +#define __SMUADX __smuadx +#define __SMLAD __smlad +#define __SMLADX __smladx +#define __SMLALD __smlald +#define __SMLALDX __smlaldx +#define __SMUSD __smusd +#define __SMUSDX __smusdx +#define __SMLSD __smlsd +#define __SMLSDX __smlsdx +#define __SMLSLD __smlsld +#define __SMLSLDX __smlsldx +#define __SEL __sel +#define __QADD __qadd +#define __QSUB __qsub + +#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \ + ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) ) + +#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \ + ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) ) + +#define __SMMLA(ARG1,ARG2,ARG3) ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \ + ((int64_t)(ARG3) << 32) ) >> 32)) + +/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ + + + +#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/ +/* IAR iccarm specific functions */ + +/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ +#include + +/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ + + + +#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/ +/* TI CCS specific functions */ + +/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ +#include + +/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ + + + +#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/ +/* GNU gcc specific functions */ + +/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3) +{ + uint32_t result; + + __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); + return(result); +} + +#define __SSAT16(ARG1,ARG2) \ +({ \ + uint32_t __RES, __ARG1 = (ARG1); \ + __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ + __RES; \ + }) + +#define __USAT16(ARG1,ARG2) \ +({ \ + uint32_t __RES, __ARG1 = (ARG1); \ + __ASM ("usat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ + __RES; \ + }) + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1) +{ + uint32_t result; + + __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1)); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1) +{ + uint32_t result; + + __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1)); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD (uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3) +{ + uint32_t result; + + __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3) +{ + uint32_t result; + + __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); + return(result); +} + +#define __SMLALD(ARG1,ARG2,ARG3) \ +({ \ + uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \ + __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ + (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ + }) + +#define __SMLALDX(ARG1,ARG2,ARG3) \ +({ \ + uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \ + __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ + (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ + }) + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD (uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3) +{ + uint32_t result; + + __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3) +{ + uint32_t result; + + __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); + return(result); +} + +#define __SMLSLD(ARG1,ARG2,ARG3) \ +({ \ + uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \ + __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ + (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ + }) + +#define __SMLSLDX(ARG1,ARG2,ARG3) \ +({ \ + uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \ + __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ + (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ + }) + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL (uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2) +{ + uint32_t result; + + __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); + return(result); +} + +#define __PKHBT(ARG1,ARG2,ARG3) \ +({ \ + uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \ + __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \ + __RES; \ + }) + +#define __PKHTB(ARG1,ARG2,ARG3) \ +({ \ + uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \ + if (ARG3 == 0) \ + __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2) ); \ + else \ + __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \ + __RES; \ + }) + +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3) +{ + int32_t result; + + __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) ); + return(result); +} + +/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ + + + +#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/ +/* TASKING carm specific functions */ + + +/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ +/* not yet supported */ +/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ + + +#endif + +/*@} end of group CMSIS_SIMD_intrinsics */ + + +#endif /* __CORE_CM4_SIMD_H */ + +#ifdef __cplusplus +} +#endif diff --git a/src/lib/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h new file mode 100644 index 000000000..139bc3c5e --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h @@ -0,0 +1,636 @@ +/**************************************************************************//** + * @file core_cmFunc.h + * @brief CMSIS Cortex-M Core Function Access Header File + * @version V3.20 + * @date 25. February 2013 + * + * @note + * + ******************************************************************************/ +/* Copyright (c) 2009 - 2013 ARM LIMITED + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - 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. + - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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 __CORE_CMFUNC_H +#define __CORE_CMFUNC_H + + +/* ########################### Core Function Access ########################### */ +/** \ingroup CMSIS_Core_FunctionInterface + \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions + @{ + */ + +#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ +/* ARM armcc specific functions */ + +#if (__ARMCC_VERSION < 400677) + #error "Please use ARM Compiler Toolchain V4.0.677 or later!" +#endif + +/* intrinsic void __enable_irq(); */ +/* intrinsic void __disable_irq(); */ + +/** \brief Get Control Register + + This function returns the content of the Control Register. + + \return Control Register value + */ +__STATIC_INLINE uint32_t __get_CONTROL(void) +{ + register uint32_t __regControl __ASM("control"); + return(__regControl); +} + + +/** \brief Set Control Register + + This function writes the given value to the Control Register. + + \param [in] control Control Register value to set + */ +__STATIC_INLINE void __set_CONTROL(uint32_t control) +{ + register uint32_t __regControl __ASM("control"); + __regControl = control; +} + + +/** \brief Get IPSR Register + + This function returns the content of the IPSR Register. + + \return IPSR Register value + */ +__STATIC_INLINE uint32_t __get_IPSR(void) +{ + register uint32_t __regIPSR __ASM("ipsr"); + return(__regIPSR); +} + + +/** \brief Get APSR Register + + This function returns the content of the APSR Register. + + \return APSR Register value + */ +__STATIC_INLINE uint32_t __get_APSR(void) +{ + register uint32_t __regAPSR __ASM("apsr"); + return(__regAPSR); +} + + +/** \brief Get xPSR Register + + This function returns the content of the xPSR Register. + + \return xPSR Register value + */ +__STATIC_INLINE uint32_t __get_xPSR(void) +{ + register uint32_t __regXPSR __ASM("xpsr"); + return(__regXPSR); +} + + +/** \brief Get Process Stack Pointer + + This function returns the current value of the Process Stack Pointer (PSP). + + \return PSP Register value + */ +__STATIC_INLINE uint32_t __get_PSP(void) +{ + register uint32_t __regProcessStackPointer __ASM("psp"); + return(__regProcessStackPointer); +} + + +/** \brief Set Process Stack Pointer + + This function assigns the given value to the Process Stack Pointer (PSP). + + \param [in] topOfProcStack Process Stack Pointer value to set + */ +__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack) +{ + register uint32_t __regProcessStackPointer __ASM("psp"); + __regProcessStackPointer = topOfProcStack; +} + + +/** \brief Get Main Stack Pointer + + This function returns the current value of the Main Stack Pointer (MSP). + + \return MSP Register value + */ +__STATIC_INLINE uint32_t __get_MSP(void) +{ + register uint32_t __regMainStackPointer __ASM("msp"); + return(__regMainStackPointer); +} + + +/** \brief Set Main Stack Pointer + + This function assigns the given value to the Main Stack Pointer (MSP). + + \param [in] topOfMainStack Main Stack Pointer value to set + */ +__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack) +{ + register uint32_t __regMainStackPointer __ASM("msp"); + __regMainStackPointer = topOfMainStack; +} + + +/** \brief Get Priority Mask + + This function returns the current state of the priority mask bit from the Priority Mask Register. + + \return Priority Mask value + */ +__STATIC_INLINE uint32_t __get_PRIMASK(void) +{ + register uint32_t __regPriMask __ASM("primask"); + return(__regPriMask); +} + + +/** \brief Set Priority Mask + + This function assigns the given value to the Priority Mask Register. + + \param [in] priMask Priority Mask + */ +__STATIC_INLINE void __set_PRIMASK(uint32_t priMask) +{ + register uint32_t __regPriMask __ASM("primask"); + __regPriMask = (priMask); +} + + +#if (__CORTEX_M >= 0x03) + +/** \brief Enable FIQ + + This function enables FIQ interrupts by clearing the F-bit in the CPSR. + Can only be executed in Privileged modes. + */ +#define __enable_fault_irq __enable_fiq + + +/** \brief Disable FIQ + + This function disables FIQ interrupts by setting the F-bit in the CPSR. + Can only be executed in Privileged modes. + */ +#define __disable_fault_irq __disable_fiq + + +/** \brief Get Base Priority + + This function returns the current value of the Base Priority register. + + \return Base Priority register value + */ +__STATIC_INLINE uint32_t __get_BASEPRI(void) +{ + register uint32_t __regBasePri __ASM("basepri"); + return(__regBasePri); +} + + +/** \brief Set Base Priority + + This function assigns the given value to the Base Priority register. + + \param [in] basePri Base Priority value to set + */ +__STATIC_INLINE void __set_BASEPRI(uint32_t basePri) +{ + register uint32_t __regBasePri __ASM("basepri"); + __regBasePri = (basePri & 0xff); +} + + +/** \brief Get Fault Mask + + This function returns the current value of the Fault Mask register. + + \return Fault Mask register value + */ +__STATIC_INLINE uint32_t __get_FAULTMASK(void) +{ + register uint32_t __regFaultMask __ASM("faultmask"); + return(__regFaultMask); +} + + +/** \brief Set Fault Mask + + This function assigns the given value to the Fault Mask register. + + \param [in] faultMask Fault Mask value to set + */ +__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask) +{ + register uint32_t __regFaultMask __ASM("faultmask"); + __regFaultMask = (faultMask & (uint32_t)1); +} + +#endif /* (__CORTEX_M >= 0x03) */ + + +#if (__CORTEX_M == 0x04) + +/** \brief Get FPSCR + + This function returns the current value of the Floating Point Status/Control register. + + \return Floating Point Status/Control register value + */ +__STATIC_INLINE uint32_t __get_FPSCR(void) +{ +#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + register uint32_t __regfpscr __ASM("fpscr"); + return(__regfpscr); +#else + return(0); +#endif +} + + +/** \brief Set FPSCR + + This function assigns the given value to the Floating Point Status/Control register. + + \param [in] fpscr Floating Point Status/Control value to set + */ +__STATIC_INLINE void __set_FPSCR(uint32_t fpscr) +{ +#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + register uint32_t __regfpscr __ASM("fpscr"); + __regfpscr = (fpscr); +#endif +} + +#endif /* (__CORTEX_M == 0x04) */ + + +#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/ +/* IAR iccarm specific functions */ + +#include + + +#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/ +/* TI CCS specific functions */ + +#include + + +#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/ +/* GNU gcc specific functions */ + +/** \brief Enable IRQ Interrupts + + This function enables IRQ interrupts by clearing the I-bit in the CPSR. + Can only be executed in Privileged modes. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void) +{ + __ASM volatile ("cpsie i" : : : "memory"); +} + + +/** \brief Disable IRQ Interrupts + + This function disables IRQ interrupts by setting the I-bit in the CPSR. + Can only be executed in Privileged modes. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void) +{ + __ASM volatile ("cpsid i" : : : "memory"); +} + + +/** \brief Get Control Register + + This function returns the content of the Control Register. + + \return Control Register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, control" : "=r" (result) ); + return(result); +} + + +/** \brief Set Control Register + + This function writes the given value to the Control Register. + + \param [in] control Control Register value to set + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control) +{ + __ASM volatile ("MSR control, %0" : : "r" (control) : "memory"); +} + + +/** \brief Get IPSR Register + + This function returns the content of the IPSR Register. + + \return IPSR Register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, ipsr" : "=r" (result) ); + return(result); +} + + +/** \brief Get APSR Register + + This function returns the content of the APSR Register. + + \return APSR Register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, apsr" : "=r" (result) ); + return(result); +} + + +/** \brief Get xPSR Register + + This function returns the content of the xPSR Register. + + \return xPSR Register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, xpsr" : "=r" (result) ); + return(result); +} + + +/** \brief Get Process Stack Pointer + + This function returns the current value of the Process Stack Pointer (PSP). + + \return PSP Register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void) +{ + register uint32_t result; + + __ASM volatile ("MRS %0, psp\n" : "=r" (result) ); + return(result); +} + + +/** \brief Set Process Stack Pointer + + This function assigns the given value to the Process Stack Pointer (PSP). + + \param [in] topOfProcStack Process Stack Pointer value to set + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack) +{ + __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp"); +} + + +/** \brief Get Main Stack Pointer + + This function returns the current value of the Main Stack Pointer (MSP). + + \return MSP Register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void) +{ + register uint32_t result; + + __ASM volatile ("MRS %0, msp\n" : "=r" (result) ); + return(result); +} + + +/** \brief Set Main Stack Pointer + + This function assigns the given value to the Main Stack Pointer (MSP). + + \param [in] topOfMainStack Main Stack Pointer value to set + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack) +{ + __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp"); +} + + +/** \brief Get Priority Mask + + This function returns the current state of the priority mask bit from the Priority Mask Register. + + \return Priority Mask value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, primask" : "=r" (result) ); + return(result); +} + + +/** \brief Set Priority Mask + + This function assigns the given value to the Priority Mask Register. + + \param [in] priMask Priority Mask + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask) +{ + __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory"); +} + + +#if (__CORTEX_M >= 0x03) + +/** \brief Enable FIQ + + This function enables FIQ interrupts by clearing the F-bit in the CPSR. + Can only be executed in Privileged modes. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void) +{ + __ASM volatile ("cpsie f" : : : "memory"); +} + + +/** \brief Disable FIQ + + This function disables FIQ interrupts by setting the F-bit in the CPSR. + Can only be executed in Privileged modes. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void) +{ + __ASM volatile ("cpsid f" : : : "memory"); +} + + +/** \brief Get Base Priority + + This function returns the current value of the Base Priority register. + + \return Base Priority register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, basepri_max" : "=r" (result) ); + return(result); +} + + +/** \brief Set Base Priority + + This function assigns the given value to the Base Priority register. + + \param [in] basePri Base Priority value to set + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value) +{ + __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory"); +} + + +/** \brief Get Fault Mask + + This function returns the current value of the Fault Mask register. + + \return Fault Mask register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void) +{ + uint32_t result; + + __ASM volatile ("MRS %0, faultmask" : "=r" (result) ); + return(result); +} + + +/** \brief Set Fault Mask + + This function assigns the given value to the Fault Mask register. + + \param [in] faultMask Fault Mask value to set + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask) +{ + __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory"); +} + +#endif /* (__CORTEX_M >= 0x03) */ + + +#if (__CORTEX_M == 0x04) + +/** \brief Get FPSCR + + This function returns the current value of the Floating Point Status/Control register. + + \return Floating Point Status/Control register value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void) +{ +#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + uint32_t result; + + /* Empty asm statement works as a scheduling barrier */ + __ASM volatile (""); + __ASM volatile ("VMRS %0, fpscr" : "=r" (result) ); + __ASM volatile (""); + return(result); +#else + return(0); +#endif +} + + +/** \brief Set FPSCR + + This function assigns the given value to the Floating Point Status/Control register. + + \param [in] fpscr Floating Point Status/Control value to set + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr) +{ +#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + /* Empty asm statement works as a scheduling barrier */ + __ASM volatile (""); + __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc"); + __ASM volatile (""); +#endif +} + +#endif /* (__CORTEX_M == 0x04) */ + + +#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/ +/* TASKING carm specific functions */ + +/* + * The CMSIS functions have been implemented as intrinsics in the compiler. + * Please use "carm -?i" to get an up to date list of all instrinsics, + * Including the CMSIS ones. + */ + +#endif + +/*@} end of CMSIS_Core_RegAccFunctions */ + + +#endif /* __CORE_CMFUNC_H */ diff --git a/src/lib/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h new file mode 100644 index 000000000..8946c2c49 --- /dev/null +++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h @@ -0,0 +1,688 @@ +/**************************************************************************//** + * @file core_cmInstr.h + * @brief CMSIS Cortex-M Core Instruction Access Header File + * @version V3.20 + * @date 05. March 2013 + * + * @note + * + ******************************************************************************/ +/* Copyright (c) 2009 - 2013 ARM LIMITED + + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - 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. + - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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 __CORE_CMINSTR_H +#define __CORE_CMINSTR_H + + +/* ########################## Core Instruction Access ######################### */ +/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface + Access to dedicated instructions + @{ +*/ + +#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ +/* ARM armcc specific functions */ + +#if (__ARMCC_VERSION < 400677) + #error "Please use ARM Compiler Toolchain V4.0.677 or later!" +#endif + + +/** \brief No Operation + + No Operation does nothing. This instruction can be used for code alignment purposes. + */ +#define __NOP __nop + + +/** \brief Wait For Interrupt + + Wait For Interrupt is a hint instruction that suspends execution + until one of a number of events occurs. + */ +#define __WFI __wfi + + +/** \brief Wait For Event + + Wait For Event is a hint instruction that permits the processor to enter + a low-power state until one of a number of events occurs. + */ +#define __WFE __wfe + + +/** \brief Send Event + + Send Event is a hint instruction. It causes an event to be signaled to the CPU. + */ +#define __SEV __sev + + +/** \brief Instruction Synchronization Barrier + + Instruction Synchronization Barrier flushes the pipeline in the processor, + so that all instructions following the ISB are fetched from cache or + memory, after the instruction has been completed. + */ +#define __ISB() __isb(0xF) + + +/** \brief Data Synchronization Barrier + + This function acts as a special kind of Data Memory Barrier. + It completes when all explicit memory accesses before this instruction complete. + */ +#define __DSB() __dsb(0xF) + + +/** \brief Data Memory Barrier + + This function ensures the apparent order of the explicit memory operations before + and after the instruction, without ensuring their completion. + */ +#define __DMB() __dmb(0xF) + + +/** \brief Reverse byte order (32 bit) + + This function reverses the byte order in integer value. + + \param [in] value Value to reverse + \return Reversed value + */ +#define __REV __rev + + +/** \brief Reverse byte order (16 bit) + + This function reverses the byte order in two unsigned short values. + + \param [in] value Value to reverse + \return Reversed value + */ +#ifndef __NO_EMBEDDED_ASM +__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value) +{ + rev16 r0, r0 + bx lr +} +#endif + +/** \brief Reverse byte order in signed short value + + This function reverses the byte order in a signed short value with sign extension to integer. + + \param [in] value Value to reverse + \return Reversed value + */ +#ifndef __NO_EMBEDDED_ASM +__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value) +{ + revsh r0, r0 + bx lr +} +#endif + + +/** \brief Rotate Right in unsigned value (32 bit) + + This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits. + + \param [in] value Value to rotate + \param [in] value Number of Bits to rotate + \return Rotated value + */ +#define __ROR __ror + + +/** \brief Breakpoint + + This function causes the processor to enter Debug state. + Debug tools can use this to investigate system state when the instruction at a particular address is reached. + + \param [in] value is ignored by the processor. + If required, a debugger can use it to store additional information about the breakpoint. + */ +#define __BKPT(value) __breakpoint(value) + + +#if (__CORTEX_M >= 0x03) + +/** \brief Reverse bit order of value + + This function reverses the bit order of the given value. + + \param [in] value Value to reverse + \return Reversed value + */ +#define __RBIT __rbit + + +/** \brief LDR Exclusive (8 bit) + + This function performs a exclusive LDR command for 8 bit value. + + \param [in] ptr Pointer to data + \return value of type uint8_t at (*ptr) + */ +#define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr)) + + +/** \brief LDR Exclusive (16 bit) + + This function performs a exclusive LDR command for 16 bit values. + + \param [in] ptr Pointer to data + \return value of type uint16_t at (*ptr) + */ +#define __LDREXH(ptr) ((uint16_t) __ldrex(ptr)) + + +/** \brief LDR Exclusive (32 bit) + + This function performs a exclusive LDR command for 32 bit values. + + \param [in] ptr Pointer to data + \return value of type uint32_t at (*ptr) + */ +#define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr)) + + +/** \brief STR Exclusive (8 bit) + + This function performs a exclusive STR command for 8 bit values. + + \param [in] value Value to store + \param [in] ptr Pointer to location + \return 0 Function succeeded + \return 1 Function failed + */ +#define __STREXB(value, ptr) __strex(value, ptr) + + +/** \brief STR Exclusive (16 bit) + + This function performs a exclusive STR command for 16 bit values. + + \param [in] value Value to store + \param [in] ptr Pointer to location + \return 0 Function succeeded + \return 1 Function failed + */ +#define __STREXH(value, ptr) __strex(value, ptr) + + +/** \brief STR Exclusive (32 bit) + + This function performs a exclusive STR command for 32 bit values. + + \param [in] value Value to store + \param [in] ptr Pointer to location + \return 0 Function succeeded + \return 1 Function failed + */ +#define __STREXW(value, ptr) __strex(value, ptr) + + +/** \brief Remove the exclusive lock + + This function removes the exclusive lock which is created by LDREX. + + */ +#define __CLREX __clrex + + +/** \brief Signed Saturate + + This function saturates a signed value. + + \param [in] value Value to be saturated + \param [in] sat Bit position to saturate to (1..32) + \return Saturated value + */ +#define __SSAT __ssat + + +/** \brief Unsigned Saturate + + This function saturates an unsigned value. + + \param [in] value Value to be saturated + \param [in] sat Bit position to saturate to (0..31) + \return Saturated value + */ +#define __USAT __usat + + +/** \brief Count leading zeros + + This function counts the number of leading zeros of a data value. + + \param [in] value Value to count the leading zeros + \return number of leading zeros in value + */ +#define __CLZ __clz + +#endif /* (__CORTEX_M >= 0x03) */ + + + +#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/ +/* IAR iccarm specific functions */ + +#include + + +#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/ +/* TI CCS specific functions */ + +#include + + +#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/ +/* GNU gcc specific functions */ + +/* Define macros for porting to both thumb1 and thumb2. + * For thumb1, use low register (r0-r7), specified by constrant "l" + * Otherwise, use general registers, specified by constrant "r" */ +#if defined (__thumb__) && !defined (__thumb2__) +#define __CMSIS_GCC_OUT_REG(r) "=l" (r) +#define __CMSIS_GCC_USE_REG(r) "l" (r) +#else +#define __CMSIS_GCC_OUT_REG(r) "=r" (r) +#define __CMSIS_GCC_USE_REG(r) "r" (r) +#endif + +/** \brief No Operation + + No Operation does nothing. This instruction can be used for code alignment purposes. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void) +{ + __ASM volatile ("nop"); +} + + +/** \brief Wait For Interrupt + + Wait For Interrupt is a hint instruction that suspends execution + until one of a number of events occurs. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void) +{ + __ASM volatile ("wfi"); +} + + +/** \brief Wait For Event + + Wait For Event is a hint instruction that permits the processor to enter + a low-power state until one of a number of events occurs. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void) +{ + __ASM volatile ("wfe"); +} + + +/** \brief Send Event + + Send Event is a hint instruction. It causes an event to be signaled to the CPU. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void) +{ + __ASM volatile ("sev"); +} + + +/** \brief Instruction Synchronization Barrier + + Instruction Synchronization Barrier flushes the pipeline in the processor, + so that all instructions following the ISB are fetched from cache or + memory, after the instruction has been completed. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void) +{ + __ASM volatile ("isb"); +} + + +/** \brief Data Synchronization Barrier + + This function acts as a special kind of Data Memory Barrier. + It completes when all explicit memory accesses before this instruction complete. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void) +{ + __ASM volatile ("dsb"); +} + + +/** \brief Data Memory Barrier + + This function ensures the apparent order of the explicit memory operations before + and after the instruction, without ensuring their completion. + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void) +{ + __ASM volatile ("dmb"); +} + + +/** \brief Reverse byte order (32 bit) + + This function reverses the byte order in integer value. + + \param [in] value Value to reverse + \return Reversed value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value) +{ +#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5) + return __builtin_bswap32(value); +#else + uint32_t result; + + __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); + return(result); +#endif +} + + +/** \brief Reverse byte order (16 bit) + + This function reverses the byte order in two unsigned short values. + + \param [in] value Value to reverse + \return Reversed value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value) +{ + uint32_t result; + + __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); + return(result); +} + + +/** \brief Reverse byte order in signed short value + + This function reverses the byte order in a signed short value with sign extension to integer. + + \param [in] value Value to reverse + \return Reversed value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value) +{ +#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) + return (short)__builtin_bswap16(value); +#else + uint32_t result; + + __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); + return(result); +#endif +} + + +/** \brief Rotate Right in unsigned value (32 bit) + + This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits. + + \param [in] value Value to rotate + \param [in] value Number of Bits to rotate + \return Rotated value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2) +{ + return (op1 >> op2) | (op1 << (32 - op2)); +} + + +/** \brief Breakpoint + + This function causes the processor to enter Debug state. + Debug tools can use this to investigate system state when the instruction at a particular address is reached. + + \param [in] value is ignored by the processor. + If required, a debugger can use it to store additional information about the breakpoint. + */ +#define __BKPT(value) __ASM volatile ("bkpt "#value) + + +#if (__CORTEX_M >= 0x03) + +/** \brief Reverse bit order of value + + This function reverses the bit order of the given value. + + \param [in] value Value to reverse + \return Reversed value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value) +{ + uint32_t result; + + __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) ); + return(result); +} + + +/** \brief LDR Exclusive (8 bit) + + This function performs a exclusive LDR command for 8 bit value. + + \param [in] ptr Pointer to data + \return value of type uint8_t at (*ptr) + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr) +{ + uint32_t result; + +#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) + __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) ); +#else + /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not + accepted by assembler. So has to use following less efficient pattern. + */ + __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" ); +#endif + return(result); +} + + +/** \brief LDR Exclusive (16 bit) + + This function performs a exclusive LDR command for 16 bit values. + + \param [in] ptr Pointer to data + \return value of type uint16_t at (*ptr) + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr) +{ + uint32_t result; + +#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) + __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) ); +#else + /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not + accepted by assembler. So has to use following less efficient pattern. + */ + __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" ); +#endif + return(result); +} + + +/** \brief LDR Exclusive (32 bit) + + This function performs a exclusive LDR command for 32 bit values. + + \param [in] ptr Pointer to data + \return value of type uint32_t at (*ptr) + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr) +{ + uint32_t result; + + __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) ); + return(result); +} + + +/** \brief STR Exclusive (8 bit) + + This function performs a exclusive STR command for 8 bit values. + + \param [in] value Value to store + \param [in] ptr Pointer to location + \return 0 Function succeeded + \return 1 Function failed + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr) +{ + uint32_t result; + + __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) ); + return(result); +} + + +/** \brief STR Exclusive (16 bit) + + This function performs a exclusive STR command for 16 bit values. + + \param [in] value Value to store + \param [in] ptr Pointer to location + \return 0 Function succeeded + \return 1 Function failed + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr) +{ + uint32_t result; + + __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) ); + return(result); +} + + +/** \brief STR Exclusive (32 bit) + + This function performs a exclusive STR command for 32 bit values. + + \param [in] value Value to store + \param [in] ptr Pointer to location + \return 0 Function succeeded + \return 1 Function failed + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr) +{ + uint32_t result; + + __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) ); + return(result); +} + + +/** \brief Remove the exclusive lock + + This function removes the exclusive lock which is created by LDREX. + + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void) +{ + __ASM volatile ("clrex" ::: "memory"); +} + + +/** \brief Signed Saturate + + This function saturates a signed value. + + \param [in] value Value to be saturated + \param [in] sat Bit position to saturate to (1..32) + \return Saturated value + */ +#define __SSAT(ARG1,ARG2) \ +({ \ + uint32_t __RES, __ARG1 = (ARG1); \ + __ASM ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ + __RES; \ + }) + + +/** \brief Unsigned Saturate + + This function saturates an unsigned value. + + \param [in] value Value to be saturated + \param [in] sat Bit position to saturate to (0..31) + \return Saturated value + */ +#define __USAT(ARG1,ARG2) \ +({ \ + uint32_t __RES, __ARG1 = (ARG1); \ + __ASM ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ + __RES; \ + }) + + +/** \brief Count leading zeros + + This function counts the number of leading zeros of a data value. + + \param [in] value Value to count the leading zeros + \return number of leading zeros in value + */ +__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value) +{ + uint32_t result; + + __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) ); + return(result); +} + +#endif /* (__CORTEX_M >= 0x03) */ + + + + +#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/ +/* TASKING carm specific functions */ + +/* + * The CMSIS functions have been implemented as intrinsics in the compiler. + * Please use "carm -?i" to get an up to date list of all intrinsics, + * Including the CMSIS ones. + */ + +#endif + +/*@}*/ /* end of group CMSIS_Core_InstructionInterface */ + +#endif /* __CORE_CMINSTR_H */ diff --git a/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a new file mode 100644 index 000000000..6898bc27d Binary files /dev/null and b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a differ diff --git a/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a new file mode 100755 index 000000000..a0185eaa9 Binary files /dev/null and b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a differ diff --git a/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a new file mode 100755 index 000000000..94525528e Binary files /dev/null and b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a differ diff --git a/src/lib/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk new file mode 100644 index 000000000..0cc1b559d --- /dev/null +++ b/src/lib/mathlib/CMSIS/library.mk @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# ARM CMSIS DSP library +# + +ifeq ($(CONFIG_ARCH),CORTEXM4F) +PREBUILT_LIB := libarm_cortexM4lf_math.a +else ifeq ($(CONFIG_ARCH),CORTEXM4) +PREBUILT_LIB := libarm_cortexM4l_math.a +else ifeq ($(CONFIG_ARCH),CORTEXM3) +PREBUILT_LIB := libarm_cortexM3l_math.a +else +$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library) +endif diff --git a/src/lib/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt new file mode 100644 index 000000000..31afac1ec --- /dev/null +++ b/src/lib/mathlib/CMSIS/license.txt @@ -0,0 +1,27 @@ +All pre-built libraries are guided by the following license: + +Copyright (C) 2009-2012 ARM Limited. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + - Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + - 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. + - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp new file mode 100644 index 000000000..f509f7081 --- /dev/null +++ b/src/lib/mathlib/math/Dcm.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 Dcm.cpp + * + * math direction cosine matrix + */ + +#include + +#include "Dcm.hpp" +#include "Quaternion.hpp" +#include "EulerAngles.hpp" +#include "Vector3.hpp" + +namespace math +{ + +Dcm::Dcm() : + Matrix(Matrix::identity(3)) +{ +} + +Dcm::Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + dcm(0, 0) = c00; + dcm(0, 1) = c01; + dcm(0, 2) = c02; + dcm(1, 0) = c10; + dcm(1, 1) = c11; + dcm(1, 2) = c12; + dcm(2, 0) = c20; + dcm(2, 1) = c21; + dcm(2, 2) = c22; +} + +Dcm::Dcm(const float data[3][3]) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + dcm(i, j) = data[i][j]; +} + +Dcm::Dcm(const float *data) : + Matrix(3, 3, data) +{ +} + +Dcm::Dcm(const Quaternion &q) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + double a = q.getA(); + double b = q.getB(); + double c = q.getC(); + double d = q.getD(); + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + dcm(0, 0) = aSq + bSq - cSq - dSq; + dcm(0, 1) = 2.0 * (b * c - a * d); + dcm(0, 2) = 2.0 * (a * c + b * d); + dcm(1, 0) = 2.0 * (b * c + a * d); + dcm(1, 1) = aSq - bSq + cSq - dSq; + dcm(1, 2) = 2.0 * (c * d - a * b); + dcm(2, 0) = 2.0 * (b * d - a * c); + dcm(2, 1) = 2.0 * (a * b + c * d); + dcm(2, 2) = aSq - bSq - cSq + dSq; +} + +Dcm::Dcm(const EulerAngles &euler) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + double cosPhi = cos(euler.getPhi()); + double sinPhi = sin(euler.getPhi()); + double cosThe = cos(euler.getTheta()); + double sinThe = sin(euler.getTheta()); + double cosPsi = cos(euler.getPsi()); + double sinPsi = sin(euler.getPsi()); + + dcm(0, 0) = cosThe * cosPsi; + dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm(1, 0) = cosThe * sinPsi; + dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm(2, 0) = -sinThe; + dcm(2, 1) = sinPhi * cosThe; + dcm(2, 2) = cosPhi * cosThe; +} + +Dcm::Dcm(const Dcm &right) : + Matrix(right) +{ +} + +Dcm::~Dcm() +{ +} + +int __EXPORT dcmTest() +{ + printf("Test DCM\t\t: "); + // default ctor + ASSERT(matrixEqual(Dcm(), + Matrix::identity(3))); + // quaternion ctor + ASSERT(matrixEqual( + Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // euler angle ctor + ASSERT(matrixEqual( + Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // rotations + Vector3 vB(1, 2, 3); + ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), + Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), + Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles( + M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); + printf("PASS\n"); + return 0; +} +} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp new file mode 100644 index 000000000..df8970d3a --- /dev/null +++ b/src/lib/mathlib/math/Dcm.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 Dcm.hpp + * + * math direction cosine matrix + */ + +//#pragma once + +#include "Vector.hpp" +#include "Matrix.hpp" + +namespace math +{ + +class Quaternion; +class EulerAngles; + +/** + * This is a Tait Bryan, Body 3-2-1 sequence. + * (yaw)-(pitch)-(roll) + * The Dcm transforms a vector in the body frame + * to the navigation frame, typically represented + * as C_nb. C_bn can be obtained through use + * of the transpose() method. + */ +class __EXPORT Dcm : public Matrix +{ +public: + /** + * default ctor + */ + Dcm(); + + /** + * scalar ctor + */ + Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22); + + /** + * data ctor + */ + Dcm(const float *data); + + /** + * array ctor + */ + Dcm(const float data[3][3]); + + /** + * quaternion ctor + */ + Dcm(const Quaternion &q); + + /** + * euler angles ctor + */ + Dcm(const EulerAngles &euler); + + /** + * copy ctor (deep) + */ + Dcm(const Dcm &right); + + /** + * dtor + */ + virtual ~Dcm(); +}; + +int __EXPORT dcmTest(); + +} // math + diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp new file mode 100644 index 000000000..e733d23bb --- /dev/null +++ b/src/lib/mathlib/math/EulerAngles.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 Vector.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "EulerAngles.hpp" +#include "Quaternion.hpp" +#include "Dcm.hpp" +#include "Vector3.hpp" + +namespace math +{ + +EulerAngles::EulerAngles() : + Vector(3) +{ + setPhi(0.0f); + setTheta(0.0f); + setPsi(0.0f); +} + +EulerAngles::EulerAngles(float phi, float theta, float psi) : + Vector(3) +{ + setPhi(phi); + setTheta(theta); + setPsi(psi); +} + +EulerAngles::EulerAngles(const Quaternion &q) : + Vector(3) +{ + (*this) = EulerAngles(Dcm(q)); +} + +EulerAngles::EulerAngles(const Dcm &dcm) : + Vector(3) +{ + setTheta(asinf(-dcm(2, 0))); + + if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { + setPhi(0.0f); + setPsi(atan2f(dcm(1, 2) - dcm(0, 1), + dcm(0, 2) + dcm(1, 1)) + getPhi()); + + } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { + setPhi(0.0f); + setPsi(atan2f(dcm(1, 2) - dcm(0, 1), + dcm(0, 2) + dcm(1, 1)) - getPhi()); + + } else { + setPhi(atan2f(dcm(2, 1), dcm(2, 2))); + setPsi(atan2f(dcm(1, 0), dcm(0, 0))); + } +} + +EulerAngles::~EulerAngles() +{ +} + +int __EXPORT eulerAnglesTest() +{ + printf("Test EulerAngles\t: "); + EulerAngles euler(0.1f, 0.2f, 0.3f); + + // test ctor + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + ASSERT(equal(euler.getPhi(), 0.1f)); + ASSERT(equal(euler.getTheta(), 0.2f)); + ASSERT(equal(euler.getPsi(), 0.3f)); + + // test dcm ctor + euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test quat ctor + euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test assignment + euler.setPhi(0.4f); + euler.setTheta(0.5f); + euler.setPsi(0.6f); + ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); + + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp new file mode 100644 index 000000000..399eecfa7 --- /dev/null +++ b/src/lib/mathlib/math/EulerAngles.hpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 Vector.h + * + * math vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class Quaternion; +class Dcm; + +class __EXPORT EulerAngles : public Vector +{ +public: + EulerAngles(); + EulerAngles(float phi, float theta, float psi); + EulerAngles(const Quaternion &q); + EulerAngles(const Dcm &dcm); + virtual ~EulerAngles(); + + // alias + void setPhi(float phi) { (*this)(0) = phi; } + void setTheta(float theta) { (*this)(1) = theta; } + void setPsi(float psi) { (*this)(2) = psi; } + + // const accessors + const float &getPhi() const { return (*this)(0); } + const float &getTheta() const { return (*this)(1); } + const float &getPsi() const { return (*this)(2); } + +}; + +int __EXPORT eulerAnglesTest(); + +} // math + diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp new file mode 100644 index 000000000..d4c892d8a --- /dev/null +++ b/src/lib/mathlib/math/Limits.cpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * 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 Limits.cpp + * + * Limiting / constrain helper functions + */ + + +#include +#include + +#include "Limits.hpp" + + +namespace math { + + +float __EXPORT min(float val1, float val2) +{ + return (val1 < val2) ? val1 : val2; +} + +int __EXPORT min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +unsigned __EXPORT min(unsigned val1, unsigned val2) +{ + return (val1 < val2) ? val1 : val2; +} + +uint64_t __EXPORT min(uint64_t val1, uint64_t val2) +{ + return (val1 < val2) ? val1 : val2; +} + +double __EXPORT min(double val1, double val2) +{ + return (val1 < val2) ? val1 : val2; +} + +float __EXPORT max(float val1, float val2) +{ + return (val1 > val2) ? val1 : val2; +} + +int __EXPORT max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + +unsigned __EXPORT max(unsigned val1, unsigned val2) +{ + return (val1 > val2) ? val1 : val2; +} + +uint64_t __EXPORT max(uint64_t val1, uint64_t val2) +{ + return (val1 > val2) ? val1 : val2; +} + +double __EXPORT max(double val1, double val2) +{ + return (val1 > val2) ? val1 : val2; +} + + +float __EXPORT constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +int __EXPORT constrain(int val, int min, int max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +double __EXPORT constrain(double val, double min, double max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +float __EXPORT radians(float degrees) +{ + return (degrees / 180.0f) * M_PI_F; +} + +double __EXPORT radians(double degrees) +{ + return (degrees / 180.0) * M_PI; +} + +float __EXPORT degrees(float radians) +{ + return (radians / M_PI_F) * 180.0f; +} + +double __EXPORT degrees(double radians) +{ + return (radians / M_PI) * 180.0; +} + +} \ No newline at end of file diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp new file mode 100644 index 000000000..fb778dd66 --- /dev/null +++ b/src/lib/mathlib/math/Limits.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 Limits.hpp + * + * Limiting / constrain helper functions + */ + +#pragma once + +#include +#include + +namespace math { + + +float __EXPORT min(float val1, float val2); + +int __EXPORT min(int val1, int val2); + +unsigned __EXPORT min(unsigned val1, unsigned val2); + +uint64_t __EXPORT min(uint64_t val1, uint64_t val2); + +double __EXPORT min(double val1, double val2); + +float __EXPORT max(float val1, float val2); + +int __EXPORT max(int val1, int val2); + +unsigned __EXPORT max(unsigned val1, unsigned val2); + +uint64_t __EXPORT max(uint64_t val1, uint64_t val2); + +double __EXPORT max(double val1, double val2); + + +float __EXPORT constrain(float val, float min, float max); + +int __EXPORT constrain(int val, int min, int max); + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); + +uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max); + +double __EXPORT constrain(double val, double min, double max); + +float __EXPORT radians(float degrees); + +double __EXPORT radians(double degrees); + +float __EXPORT degrees(float radians); + +double __EXPORT degrees(double radians); + +} \ No newline at end of file diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp new file mode 100644 index 000000000..ebd1aeda3 --- /dev/null +++ b/src/lib/mathlib/math/Matrix.cpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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 Matrix.cpp + * + * matrix code + */ + +#include "test/test.hpp" +#include + +#include "Matrix.hpp" + +namespace math +{ + +static const float data_testA[] = { + 1, 2, 3, + 4, 5, 6 +}; +static Matrix testA(2, 3, data_testA); + +static const float data_testB[] = { + 0, 1, 3, + 7, -1, 2 +}; +static Matrix testB(2, 3, data_testB); + +static const float data_testC[] = { + 0, 1, + 2, 1, + 3, 2 +}; +static Matrix testC(3, 2, data_testC); + +static const float data_testD[] = { + 0, 1, 2, + 2, 1, 4, + 5, 2, 0 +}; +static Matrix testD(3, 3, data_testD); + +static const float data_testE[] = { + 1, -1, 2, + 0, 2, 3, + 2, -1, 1 +}; +static Matrix testE(3, 3, data_testE); + +static const float data_testF[] = { + 3.777e006f, 2.915e007f, 0.000e000f, + 2.938e007f, 2.267e008f, 0.000e000f, + 0.000e000f, 0.000e000f, 6.033e008f +}; +static Matrix testF(3, 3, data_testF); + +int __EXPORT matrixTest() +{ + matrixAddTest(); + matrixSubTest(); + matrixMultTest(); + matrixInvTest(); + matrixDivTest(); + return 0; +} + +int matrixAddTest() +{ + printf("Test Matrix Add\t\t: "); + Matrix r = testA + testB; + float data_test[] = { + 1.0f, 3.0f, 6.0f, + 11.0f, 4.0f, 8.0f + }; + ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixSubTest() +{ + printf("Test Matrix Sub\t\t: "); + Matrix r = testA - testB; + float data_test[] = { + 1.0f, 1.0f, 0.0f, + -3.0f, 6.0f, 4.0f + }; + ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixMultTest() +{ + printf("Test Matrix Mult\t: "); + Matrix r = testC * testB; + float data_test[] = { + 7.0f, -1.0f, 2.0f, + 7.0f, 1.0f, 8.0f, + 14.0f, 1.0f, 13.0f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixInvTest() +{ + printf("Test Matrix Inv\t\t: "); + Matrix origF = testF; + Matrix r = testF.inverse(); + float data_test[] = { + -0.0012518f, 0.0001610f, 0.0000000f, + 0.0001622f, -0.0000209f, 0.0000000f, + 0.0000000f, 0.0000000f, 1.6580e-9f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + // make sure F in unchanged + ASSERT(matrixEqual(origF, testF)); + printf("PASS\n"); + return 0; +} + +int matrixDivTest() +{ + printf("Test Matrix Div\t\t: "); + Matrix r = testD / testE; + float data_test[] = { + 0.2222222f, 0.5555556f, -0.1111111f, + 0.0f, 1.0f, 1.0, + -4.1111111f, 1.2222222f, 4.5555556f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +bool matrixEqual(const Matrix &a, const Matrix &b, float eps) +{ + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + + } else if (a.getCols() != b.getCols()) { + printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) + for (size_t j = 0; j < a.getCols(); j++) { + if (!equal(a(i, j), b(i, j), eps)) { + printf("element mismatch (%d, %d)\n", i, j); + ret = false; + } + } + + return ret; +} + +} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp new file mode 100644 index 000000000..f19db15ec --- /dev/null +++ b/src/lib/mathlib/math/Matrix.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 Matrix.h + * + * matrix code + */ + +#pragma once + +#include + +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) +#include "arm/Matrix.hpp" +#else +#include "generic/Matrix.hpp" +#endif + +namespace math +{ +class Matrix; +int matrixTest(); +int matrixAddTest(); +int matrixSubTest(); +int matrixMultTest(); +int matrixInvTest(); +int matrixDivTest(); +int matrixArmTest(); +bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); +} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp new file mode 100644 index 000000000..02fec4ca6 --- /dev/null +++ b/src/lib/mathlib/math/Quaternion.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 Quaternion.cpp + * + * math vector + */ + +#include "test/test.hpp" + + +#include "Quaternion.hpp" +#include "Dcm.hpp" +#include "EulerAngles.hpp" + +namespace math +{ + +Quaternion::Quaternion() : + Vector(4) +{ + setA(1.0f); + setB(0.0f); + setC(0.0f); + setD(0.0f); +} + +Quaternion::Quaternion(float a, float b, + float c, float d) : + Vector(4) +{ + setA(a); + setB(b); + setC(c); + setD(d); +} + +Quaternion::Quaternion(const float *data) : + Vector(4, data) +{ +} + +Quaternion::Quaternion(const Vector &v) : + Vector(v) +{ +} + +Quaternion::Quaternion(const Dcm &dcm) : + Vector(4) +{ + // avoiding singularities by not using + // division equations + setA(0.5 * sqrt(1.0 + + double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); + setB(0.5 * sqrt(1.0 + + double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); + setC(0.5 * sqrt(1.0 + + double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); + setD(0.5 * sqrt(1.0 + + double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); +} + +Quaternion::Quaternion(const EulerAngles &euler) : + Vector(4) +{ + double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); + double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); + double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); + double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); + double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); + double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); + setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); +} + +Quaternion::Quaternion(const Quaternion &right) : + Vector(right) +{ +} + +Quaternion::~Quaternion() +{ +} + +Vector Quaternion::derivative(const Vector &w) +{ +#ifdef QUATERNION_ASSERT + ASSERT(w.getRows() == 3); +#endif + float dataQ[] = { + getA(), -getB(), -getC(), -getD(), + getB(), getA(), -getD(), getC(), + getC(), getD(), getA(), -getB(), + getD(), -getC(), getB(), getA() + }; + Vector v(4); + v(0) = 0.0f; + v(1) = w(0); + v(2) = w(1); + v(3) = w(2); + Matrix Q(4, 4, dataQ); + return Q * v * 0.5f; +} + +int __EXPORT quaternionTest() +{ + printf("Test Quaternion\t\t: "); + // test default ctor + Quaternion q; + ASSERT(equal(q.getA(), 1.0f)); + ASSERT(equal(q.getB(), 0.0f)); + ASSERT(equal(q.getC(), 0.0f)); + ASSERT(equal(q.getD(), 0.0f)); + // test float ctor + q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); + ASSERT(equal(q.getA(), 0.1825742f)); + ASSERT(equal(q.getB(), 0.3651484f)); + ASSERT(equal(q.getC(), 0.5477226f)); + ASSERT(equal(q.getD(), 0.7302967f)); + // test euler ctor + q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); + // test dcm ctor + q = Quaternion(Dcm()); + ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); + // TODO test derivative + // test accessors + q.setA(0.1f); + q.setB(0.2f); + q.setC(0.3f); + q.setD(0.4f); + ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp new file mode 100644 index 000000000..048a55d33 --- /dev/null +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 Quaternion.hpp + * + * math quaternion lib + */ + +#pragma once + +#include "Vector.hpp" +#include "Matrix.hpp" + +namespace math +{ + +class Dcm; +class EulerAngles; + +class __EXPORT Quaternion : public Vector +{ +public: + + /** + * default ctor + */ + Quaternion(); + + /** + * ctor from floats + */ + Quaternion(float a, float b, float c, float d); + + /** + * ctor from data + */ + Quaternion(const float *data); + + /** + * ctor from Vector + */ + Quaternion(const Vector &v); + + /** + * ctor from EulerAngles + */ + Quaternion(const EulerAngles &euler); + + /** + * ctor from Dcm + */ + Quaternion(const Dcm &dcm); + + /** + * deep copy ctor + */ + Quaternion(const Quaternion &right); + + /** + * dtor + */ + virtual ~Quaternion(); + + /** + * derivative + */ + Vector derivative(const Vector &w); + + /** + * accessors + */ + void setA(float a) { (*this)(0) = a; } + void setB(float b) { (*this)(1) = b; } + void setC(float c) { (*this)(2) = c; } + void setD(float d) { (*this)(3) = d; } + const float &getA() const { return (*this)(0); } + const float &getB() const { return (*this)(1); } + const float &getC() const { return (*this)(2); } + const float &getD() const { return (*this)(3); } +}; + +int __EXPORT quaternionTest(); +} // math + diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp new file mode 100644 index 000000000..35158a396 --- /dev/null +++ b/src/lib/mathlib/math/Vector.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector.hpp" + +namespace math +{ + +static const float data_testA[] = {1, 3}; +static const float data_testB[] = {4, 1}; + +static Vector testA(2, data_testA); +static Vector testB(2, data_testB); + +int __EXPORT vectorTest() +{ + vectorAddTest(); + vectorSubTest(); + return 0; +} + +int vectorAddTest() +{ + printf("Test Vector Add\t\t: "); + Vector r = testA + testB; + float data_test[] = {5.0f, 4.0f}; + ASSERT(vectorEqual(Vector(2, data_test), r)); + printf("PASS\n"); + return 0; +} + +int vectorSubTest() +{ + printf("Test Vector Sub\t\t: "); + Vector r(2); + r = testA - testB; + float data_test[] = { -3.0f, 2.0f}; + ASSERT(vectorEqual(Vector(2, data_test), r)); + printf("PASS\n"); + return 0; +} + +bool vectorEqual(const Vector &a, const Vector &b, float eps) +{ + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) { + if (!equal(a(i), b(i), eps)) { + printf("element mismatch (%d)\n", i); + ret = false; + } + } + + return ret; +} + +} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp new file mode 100644 index 000000000..73de793d5 --- /dev/null +++ b/src/lib/mathlib/math/Vector.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 Vector.h + * + * math vector + */ + +#pragma once + +#include + +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) +#include "arm/Vector.hpp" +#else +#include "generic/Vector.hpp" +#endif + +namespace math +{ +class Vector; +int __EXPORT vectorTest(); +int __EXPORT vectorAddTest(); +int __EXPORT vectorSubTest(); +bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); +} // math diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp new file mode 100644 index 000000000..68e741817 --- /dev/null +++ b/src/lib/mathlib/math/Vector2f.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 Vector2f.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector2f.hpp" + +namespace math +{ + +Vector2f::Vector2f() : + Vector(2) +{ +} + +Vector2f::Vector2f(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 2); +#endif +} + +Vector2f::Vector2f(float x, float y) : + Vector(2) +{ + setX(x); + setY(y); +} + +Vector2f::Vector2f(const float *data) : + Vector(2, data) +{ +} + +Vector2f::~Vector2f() +{ +} + +float Vector2f::cross(const Vector2f &b) const +{ + const Vector2f &a = *this; + return a(0)*b(1) - a(1)*b(0); +} + +float Vector2f::operator %(const Vector2f &v) const +{ + return cross(v); +} + +float Vector2f::operator *(const Vector2f &v) const +{ + return dot(v); +} + +int __EXPORT vector2fTest() +{ + printf("Test Vector2f\t\t: "); + // test float ctor + Vector2f v(1, 2); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp new file mode 100644 index 000000000..ecd62e81c --- /dev/null +++ b/src/lib/mathlib/math/Vector2f.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 Vector2f.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector2f : + public Vector +{ +public: + Vector2f(); + Vector2f(const Vector &right); + Vector2f(float x, float y); + Vector2f(const float *data); + virtual ~Vector2f(); + float cross(const Vector2f &b) const; + float operator %(const Vector2f &v) const; + float operator *(const Vector2f &v) const; + inline Vector2f operator*(const float &right) const { + return Vector::operator*(right); + } + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } +}; + +class __EXPORT Vector2 : + public Vector2f +{ +}; + +int __EXPORT vector2fTest(); +} // math + diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp new file mode 100644 index 000000000..dcb85600e --- /dev/null +++ b/src/lib/mathlib/math/Vector3.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector3.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector3.hpp" + +namespace math +{ + +Vector3::Vector3() : + Vector(3) +{ +} + +Vector3::Vector3(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 3); +#endif +} + +Vector3::Vector3(float x, float y, float z) : + Vector(3) +{ + setX(x); + setY(y); + setZ(z); +} + +Vector3::Vector3(const float *data) : + Vector(3, data) +{ +} + +Vector3::~Vector3() +{ +} + +Vector3 Vector3::cross(const Vector3 &b) const +{ + const Vector3 &a = *this; + Vector3 result; + result(0) = a(1) * b(2) - a(2) * b(1); + result(1) = a(2) * b(0) - a(0) * b(2); + result(2) = a(0) * b(1) - a(1) * b(0); + return result; +} + +int __EXPORT vector3Test() +{ + printf("Test Vector3\t\t: "); + // test float ctor + Vector3 v(1, 2, 3); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + ASSERT(equal(v(2), 3)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp new file mode 100644 index 000000000..568d9669a --- /dev/null +++ b/src/lib/mathlib/math/Vector3.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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 Vector3.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector3 : + public Vector +{ +public: + Vector3(); + Vector3(const Vector &right); + Vector3(float x, float y, float z); + Vector3(const float *data); + virtual ~Vector3(); + Vector3 cross(const Vector3 &b) const; + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + void setZ(float z) { (*this)(2) = z; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } + const float &getZ() const { return (*this)(2); } +}; + +class __EXPORT Vector3f : + public Vector3 +{ +}; + +int __EXPORT vector3Test(); +} // math + diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/src/lib/mathlib/math/arm/Matrix.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 Matrix.cpp + * + * matrix code + */ + +#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp new file mode 100644 index 000000000..715fd3a5e --- /dev/null +++ b/src/lib/mathlib/math/arm/Matrix.hpp @@ -0,0 +1,292 @@ +/**************************************************************************** + * + * 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 Matrix.h + * + * matrix code + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include + +#include "../Vector.hpp" +#include "../Matrix.hpp" + +// arm specific +#include "../../CMSIS/Include/arm_math.h" + +namespace math +{ + +class __EXPORT Matrix +{ +public: + // constructor + Matrix(size_t rows, size_t cols) : + _matrix() { + arm_mat_init_f32(&_matrix, + rows, cols, + (float *)calloc(rows * cols, sizeof(float))); + } + Matrix(size_t rows, size_t cols, const float *data) : + _matrix() { + arm_mat_init_f32(&_matrix, + rows, cols, + (float *)malloc(rows * cols * sizeof(float))); + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Matrix() { + delete [] _matrix.pData; + } + // copy constructor (deep) + Matrix(const Matrix &right) : + _matrix() { + arm_mat_init_f32(&_matrix, + right.getRows(), right.getCols(), + (float *)malloc(right.getRows()* + right.getCols()*sizeof(float))); + memcpy(getData(), right.getData(), + getSize()); + } + // assignment + inline Matrix &operator=(const Matrix &right) { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i, size_t j) { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + inline const float &operator()(size_t i, size_t j) const { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + float sig; + int exp; + float num = (*this)(i, j); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + } + // boolean ops + inline bool operator==(const Matrix &right) const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) + return false; + } + } + + return true; + } + // scalar ops + inline Matrix operator+(float right) const { + Matrix result(getRows(), getCols()); + arm_offset_f32((float *)getData(), right, + (float *)result.getData(), getRows()*getCols()); + return result; + } + inline Matrix operator-(float right) const { + Matrix result(getRows(), getCols()); + arm_offset_f32((float *)getData(), -right, + (float *)result.getData(), getRows()*getCols()); + return result; + } + inline Matrix operator*(float right) const { + Matrix result(getRows(), getCols()); + arm_mat_scale_f32(&_matrix, right, + &(result._matrix)); + return result; + } + inline Matrix operator/(float right) const { + Matrix result(getRows(), getCols()); + arm_mat_scale_f32(&_matrix, 1.0f / right, + &(result._matrix)); + return result; + } + // vector ops + inline Vector operator*(const Vector &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix resultMat = (*this) * + Matrix(right.getRows(), 1, right.getData()); + return Vector(getRows(), resultMat.getData()); + } + // matrix ops + inline Matrix operator+(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + arm_mat_add_f32(&_matrix, &(right._matrix), + &(result._matrix)); + return result; + } + inline Matrix operator-(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + arm_mat_sub_f32(&_matrix, &(right._matrix), + &(result._matrix)); + return result; + } + inline Matrix operator*(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix result(getRows(), right.getCols()); + arm_mat_mult_f32(&_matrix, &(right._matrix), + &(result._matrix)); + return result; + } + inline Matrix operator/(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(right.getRows() == right.getCols()); + ASSERT(getCols() == right.getCols()); +#endif + return (*this) * right.inverse(); + } + // other functions + inline Matrix transpose() const { + Matrix result(getCols(), getRows()); + arm_mat_trans_f32(&_matrix, &(result._matrix)); + return result; + } + inline void swapRows(size_t a, size_t b) { + if (a == b) return; + + for (size_t j = 0; j < getCols(); j++) { + float tmp = (*this)(a, j); + (*this)(a, j) = (*this)(b, j); + (*this)(b, j) = tmp; + } + } + inline void swapCols(size_t a, size_t b) { + if (a == b) return; + + for (size_t i = 0; i < getRows(); i++) { + float tmp = (*this)(i, a); + (*this)(i, a) = (*this)(i, b); + (*this)(i, b) = tmp; + } + } + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == getCols()); +#endif + Matrix result(getRows(), getCols()); + Matrix work = (*this); + arm_mat_inverse_f32(&(work._matrix), + &(result._matrix)); + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + (*this)(i, j) = val; + } + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _matrix.numRows; } + inline size_t getCols() const { return _matrix.numCols; } + inline static Matrix identity(size_t size) { + Matrix result(size, size); + + for (size_t i = 0; i < size; i++) { + result(i, i) = 1.0f; + } + + return result; + } + inline static Matrix zero(size_t size) { + Matrix result(size, size); + result.setAll(0.0f); + return result; + } + inline static Matrix zero(size_t m, size_t n) { + Matrix result(m, n); + result.setAll(0.0f); + return result; + } +protected: + inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } + inline float *getData() { return _matrix.pData; } + inline const float *getData() const { return _matrix.pData; } + inline void setData(float *data) { _matrix.pData = data; } +private: + arm_matrix_instance_f32 _matrix; +}; + +} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/src/lib/mathlib/math/arm/Vector.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 Vector.cpp + * + * math vector + */ + +#include "Vector.hpp" diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp new file mode 100644 index 000000000..4155800e8 --- /dev/null +++ b/src/lib/mathlib/math/arm/Vector.hpp @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * 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 Vector.h + * + * math vector + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "../Vector.hpp" +#include "../test/test.hpp" + +// arm specific +#include "../../CMSIS/Include/arm_math.h" + +namespace math +{ + +class __EXPORT Vector +{ +public: + // constructor + Vector(size_t rows) : + _rows(rows), + _data((float *)calloc(rows, sizeof(float))) { + } + Vector(size_t rows, const float *data) : + _rows(rows), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Vector() { + delete [] getData(); + } + // copy constructor (deep) + Vector(const Vector &right) : + _rows(right.getRows()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Vector &operator=(const Vector &right) { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i) { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + inline const float &operator()(size_t i) const { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + float sig; + int exp; + float num = (*this)(i); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + // boolean ops + inline bool operator==(const Vector &right) const { + for (size_t i = 0; i < getRows(); i++) { + if (fabsf(((*this)(i) - right(i))) > 1e-30f) + return false; + } + + return true; + } + // scalar ops + inline Vector operator+(float right) const { + Vector result(getRows()); + arm_offset_f32((float *)getData(), + right, result.getData(), + getRows()); + return result; + } + inline Vector operator-(float right) const { + Vector result(getRows()); + arm_offset_f32((float *)getData(), + -right, result.getData(), + getRows()); + return result; + } + inline Vector operator*(float right) const { + Vector result(getRows()); + arm_scale_f32((float *)getData(), + right, result.getData(), + getRows()); + return result; + } + inline Vector operator/(float right) const { + Vector result(getRows()); + arm_scale_f32((float *)getData(), + 1.0f / right, result.getData(), + getRows()); + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + arm_add_f32((float *)getData(), + (float *)right.getData(), + result.getData(), + getRows()); + return result; + } + inline Vector operator-(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + arm_sub_f32((float *)getData(), + (float *)right.getData(), + result.getData(), + getRows()); + return result; + } + inline Vector operator-(void) const { + Vector result(getRows()); + arm_negate_f32((float *)getData(), + result.getData(), + getRows()); + return result; + } + // other functions + inline float dot(const Vector &right) const { + float result = 0; + arm_dot_prod_f32((float *)getData(), + (float *)right.getData(), + getRows(), + &result); + return result; + } + inline float norm() const { + return sqrtf(dot(*this)); + } + inline float length() const { + return norm(); + } + inline Vector unit() const { + return (*this) / norm(); + } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } + inline const float *getData() const { return _data; } +protected: + inline size_t getSize() const { return sizeof(float) * getRows(); } + inline float *getData() { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + float *_data; +}; + +} // math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp new file mode 100644 index 000000000..efb17225d --- /dev/null +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * 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 LowPassFilter.cpp +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall + +#include "LowPassFilter2p.hpp" +#include "math.h" + +namespace math +{ + +void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) +{ + _cutoff_freq = cutoff_freq; + float fr = sample_freq/_cutoff_freq; + float ohm = tanf(M_PI_F/fr); + float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; + _b0 = ohm*ohm/c; + _b1 = 2.0f*_b0; + _b2 = _b0; + _a1 = 2.0f*(ohm*ohm-1.0f)/c; + _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; +} + +float LowPassFilter2p::apply(float sample) +{ + // do the filtering + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; + if (isnan(delay_element_0) || isinf(delay_element_0)) { + // don't allow bad values to propogate via the filter + delay_element_0 = sample; + } + float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + // return the value. Should be no need to check limits + return output; +} + +} // namespace math + diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp new file mode 100644 index 000000000..208ec98d4 --- /dev/null +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * 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 LowPassFilter.h +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall +/// Adapted for PX4 by Andrew Tridgell + +#pragma once + +namespace math +{ +class __EXPORT LowPassFilter2p +{ +public: + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + _delay_element_1 = _delay_element_2 = 0; + } + + // change parameters + void set_cutoff_frequency(float sample_freq, float cutoff_freq); + + // apply - Add a new raw value to the filter + // and retrieve the filtered result + float apply(float sample); + + // return the cutoff frequency + float get_cutoff_freq(void) const { + return _cutoff_freq; + } + +private: + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 +}; + +} // namespace math diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk new file mode 100644 index 000000000..fe92c8c70 --- /dev/null +++ b/src/lib/mathlib/math/filter/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. +# +############################################################################ + +# +# filter library +# +SRCS = LowPassFilter2p.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/src/lib/mathlib/math/generic/Matrix.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 Matrix.cpp + * + * matrix code + */ + +#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp new file mode 100644 index 000000000..5601a3447 --- /dev/null +++ b/src/lib/mathlib/math/generic/Matrix.hpp @@ -0,0 +1,437 @@ +/**************************************************************************** + * + * 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 Matrix.h + * + * matrix code + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include + +#include "../Vector.hpp" +#include "../Matrix.hpp" + +namespace math +{ + +class __EXPORT Matrix +{ +public: + // constructor + Matrix(size_t rows, size_t cols) : + _rows(rows), + _cols(cols), + _data((float *)calloc(rows *cols, sizeof(float))) { + } + Matrix(size_t rows, size_t cols, const float *data) : + _rows(rows), + _cols(cols), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Matrix() { + delete [] getData(); + } + // copy constructor (deep) + Matrix(const Matrix &right) : + _rows(right.getRows()), + _cols(right.getCols()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Matrix &operator=(const Matrix &right) { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i, size_t j) { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + inline const float &operator()(size_t i, size_t j) const { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + float sig; + int exp; + float num = (*this)(i, j); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + } + // boolean ops + inline bool operator==(const Matrix &right) const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) + return false; + } + } + + return true; + } + // scalar ops + inline Matrix operator+(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) + right; + } + } + + return result; + } + inline Matrix operator-(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) - right; + } + } + + return result; + } + inline Matrix operator*(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) * right; + } + } + + return result; + } + inline Matrix operator/(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) / right; + } + } + + return result; + } + // vector ops + inline Vector operator*(const Vector &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i) += (*this)(i, j) * right(j); + } + } + + return result; + } + // matrix ops + inline Matrix operator+(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) + right(i, j); + } + } + + return result; + } + inline Matrix operator-(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) - right(i, j); + } + } + + return result; + } + inline Matrix operator*(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix result(getRows(), right.getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < right.getCols(); j++) { + for (size_t k = 0; k < right.getRows(); k++) { + result(i, j) += (*this)(i, k) * right(k, j); + } + } + } + + return result; + } + inline Matrix operator/(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(right.getRows() == right.getCols()); + ASSERT(getCols() == right.getCols()); +#endif + return (*this) * right.inverse(); + } + // other functions + inline Matrix transpose() const { + Matrix result(getCols(), getRows()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(j, i) = (*this)(i, j); + } + } + + return result; + } + inline void swapRows(size_t a, size_t b) { + if (a == b) return; + + for (size_t j = 0; j < getCols(); j++) { + float tmp = (*this)(a, j); + (*this)(a, j) = (*this)(b, j); + (*this)(b, j) = tmp; + } + } + inline void swapCols(size_t a, size_t b) { + if (a == b) return; + + for (size_t i = 0; i < getRows(); i++) { + float tmp = (*this)(i, a); + (*this)(i, a) = (*this)(i, b); + (*this)(i, b) = tmp; + } + } + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == getCols()); +#endif + size_t N = getRows(); + Matrix L = identity(N); + const Matrix &A = (*this); + Matrix U = A; + Matrix P = identity(N); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n, n)) < 1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i = 0; i < N; i++) { + if (i == n) continue; + + //printf("\ttrying row %d\n",i); + if (fabsf(U(i, n)) > 1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabsf(U(n, n)) < 1e-8f) { + return Matrix::zero(n); + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < N; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < N; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < N; c++) { + // for all rows of L + for (size_t i = 0; i < N; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i, c) -= L(i, j) * Y(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + + // for all columns of X + for (size_t c = 0; c < N; c++) { + // for all rows of U + for (size_t k = 0; k < N; k++) { + // have to go in reverse order + size_t i = N - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i, c) -= U(i, j) * X(j, c); + } + + // divide by the factor + // on current + // term to be solved + X(i, c) /= U(i, i); + } + } + + //printf("X:\n"); X.print(); + return X; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + (*this)(i, j) = val; + } + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } + inline size_t getCols() const { return _cols; } + inline static Matrix identity(size_t size) { + Matrix result(size, size); + + for (size_t i = 0; i < size; i++) { + result(i, i) = 1.0f; + } + + return result; + } + inline static Matrix zero(size_t size) { + Matrix result(size, size); + result.setAll(0.0f); + return result; + } + inline static Matrix zero(size_t m, size_t n) { + Matrix result(m, n); + result.setAll(0.0f); + return result; + } +protected: + inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } + inline float *getData() { return _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + size_t _cols; + float *_data; +}; + +} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/src/lib/mathlib/math/generic/Vector.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 Vector.cpp + * + * math vector + */ + +#include "Vector.hpp" diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp new file mode 100644 index 000000000..8cfdc676d --- /dev/null +++ b/src/lib/mathlib/math/generic/Vector.hpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * 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 Vector.h + * + * math vector + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "../Vector.hpp" + +namespace math +{ + +class __EXPORT Vector +{ +public: + // constructor + Vector(size_t rows) : + _rows(rows), + _data((float *)calloc(rows, sizeof(float))) { + } + Vector(size_t rows, const float *data) : + _rows(rows), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Vector() { + delete [] getData(); + } + // copy constructor (deep) + Vector(const Vector &right) : + _rows(right.getRows()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Vector &operator=(const Vector &right) { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i) { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + inline const float &operator()(size_t i) const { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + float sig; + int exp; + float num = (*this)(i); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + // boolean ops + inline bool operator==(const Vector &right) const { + for (size_t i = 0; i < getRows(); i++) { + if (fabsf(((*this)(i) - right(i))) > 1e-30f) + return false; + } + + return true; + } + // scalar ops + inline Vector operator+(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right; + } + + return result; + } + inline Vector operator-(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right; + } + + return result; + } + inline Vector operator*(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) * right; + } + + return result; + } + inline Vector operator/(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) / right; + } + + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right(i); + } + + return result; + } + inline Vector operator-(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right(i); + } + + return result; + } + inline Vector operator-(void) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = -((*this)(i)); + } + + return result; + } + // other functions + inline float dot(const Vector &right) const { + float result = 0; + + for (size_t i = 0; i < getRows(); i++) { + result += (*this)(i) * (*this)(i); + } + + return result; + } + inline float norm() const { + return sqrtf(dot(*this)); + } + inline float length() const { + return norm(); + } + inline Vector unit() const { + return (*this) / norm(); + } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } +protected: + inline size_t getSize() const { return sizeof(float) * getRows(); } + inline float *getData() { return _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + float *_data; +}; + +} // math diff --git a/src/lib/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf new file mode 100644 index 000000000..eb67a4bfc Binary files /dev/null and b/src/lib/mathlib/math/nasa_rotation_def.pdf differ diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp new file mode 100644 index 000000000..2fa2f7e7c --- /dev/null +++ b/src/lib/mathlib/math/test/test.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 test.cpp + * + * Test library code + */ + +#include +#include +#include + +#include "test.hpp" + +bool __EXPORT equal(float a, float b, float epsilon) +{ + float diff = fabsf(a - b); + + if (diff > epsilon) { + printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + + } else return true; +} + +void __EXPORT float2SigExp( + const float &num, + float &sig, + int &exp) +{ + if (isnan(num) || isinf(num)) { + sig = 0.0f; + exp = -99; + return; + } + + if (fabsf(num) < 1.0e-38f) { + sig = 0; + exp = 0; + return; + } + + exp = log10f(fabsf(num)); + + if (exp > 0) { + exp = ceil(exp); + + } else { + exp = floor(exp); + } + + sig = num; + + // cheap power since it is integer + if (exp > 0) { + for (int i = 0; i < abs(exp); i++) sig /= 10; + + } else { + for (int i = 0; i < abs(exp); i++) sig *= 10; + } +} + + diff --git a/src/lib/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp new file mode 100644 index 000000000..2027bb827 --- /dev/null +++ b/src/lib/mathlib/math/test/test.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 test.hpp + * + * Controller library code + */ + +#pragma once + +//#include +//#include +//#include + +bool equal(float a, float b, float eps = 1e-5); +void float2SigExp( + const float &num, + float &sig, + int &exp); diff --git a/src/lib/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce new file mode 100644 index 000000000..c3fba4729 --- /dev/null +++ b/src/lib/mathlib/math/test_math.sce @@ -0,0 +1,63 @@ +clc +clear +function out = float_truncate(in, digits) + out = round(in*10^digits) + out = out/10^digits +endfunction + +phi = 0.1 +theta = 0.2 +psi = 0.3 + +cosPhi = cos(phi) +cosPhi_2 = cos(phi/2) +sinPhi = sin(phi) +sinPhi_2 = sin(phi/2) + +cosTheta = cos(theta) +cosTheta_2 = cos(theta/2) +sinTheta = sin(theta) +sinTheta_2 = sin(theta/2) + +cosPsi = cos(psi) +cosPsi_2 = cos(psi/2) +sinPsi = sin(psi) +sinPsi_2 = sin(psi/2) + +C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; + cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; + -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] + +disp(C_nb) +//C_nb = float_truncate(C_nb,3) +//disp(C_nb) + +theta = asin(-C_nb(3,1)) +phi = atan(C_nb(3,2), C_nb(3,3)) +psi = atan(C_nb(2,1), C_nb(1,1)) +printf('phi %f\n', phi) +printf('theta %f\n', theta) +printf('psi %f\n', psi) + +q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; + sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; + cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; + cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] + +//q = float_truncate(q,3) + +a = q(1) +b = q(2) +c = q(3) +d = q(4) +printf('q: %f %f %f %f\n', a, b, c, d) +a2 = a*a +b2 = b*b +c2 = c*c +d2 = d*d + +C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); + 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); + 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] + +disp(C2_nb) diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h new file mode 100644 index 000000000..40ffb22bc --- /dev/null +++ b/src/lib/mathlib/mathlib.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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 mathlib.h + * + * Common header for mathlib exports. + */ + +#ifdef __cplusplus + +#pragma once + +#include "math/Dcm.hpp" +#include "math/EulerAngles.hpp" +#include "math/Matrix.hpp" +#include "math/Quaternion.hpp" +#include "math/Vector.hpp" +#include "math/Vector3.hpp" +#include "math/Vector2f.hpp" +#include "math/Limits.hpp" + +#endif + +#ifdef CONFIG_ARCH_ARM + +#include "CMSIS/Include/arm_math.h" + +#endif \ No newline at end of file diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk new file mode 100644 index 000000000..2146a1413 --- /dev/null +++ b/src/lib/mathlib/module.mk @@ -0,0 +1,62 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Math library +# +SRCS = math/test/test.cpp \ + math/Vector.cpp \ + math/Vector2f.cpp \ + math/Vector3.cpp \ + math/EulerAngles.cpp \ + math/Quaternion.cpp \ + math/Dcm.cpp \ + math/Matrix.cpp \ + math/Limits.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config + +ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) +INCLUDE_DIRS += math/arm +SRCS += math/arm/Vector.cpp \ + math/arm/Matrix.cpp +else +#INCLUDE_DIRS += math/generic +#SRCS += math/generic/Vector.cpp \ +# math/generic/Matrix.cpp +endif diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b6217a414..ddd2e23d9 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -116,7 +116,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 9c0720aa5..46dc1bec2 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -58,7 +58,7 @@ #include extern "C" { -#include +#include } #include "../blocks.hpp" diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h deleted file mode 100644 index 8f39acd9d..000000000 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h +++ /dev/null @@ -1,264 +0,0 @@ -/**************************************************************************//** - * @file ARMCM3.h - * @brief CMSIS Core Peripheral Access Layer Header File for - * ARMCM3 Device Series - * @version V1.07 - * @date 30. January 2012 - * - * @note - * Copyright (C) 2012 ARM Limited. All rights reserved. - * - * @par - * ARM Limited (ARM) is supplying this software for use with Cortex-M - * processor based microcontrollers. This file can be freely distributed - * within development tools that are supporting such ARM based processors. - * - * @par - * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED - * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. - * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR - * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. - * - ******************************************************************************/ - -#ifndef ARMCM3_H -#define ARMCM3_H - -#ifdef __cplusplus -extern "C" { -#endif - - -/* ------------------------- Interrupt Number Definition ------------------------ */ - -typedef enum IRQn -{ -/* ------------------- Cortex-M3 Processor Exceptions Numbers ------------------- */ - NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */ - HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */ - MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */ - BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */ - UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */ - SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */ - DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */ - PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */ - SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */ - -/* ---------------------- ARMCM3 Specific Interrupt Numbers --------------------- */ - WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */ - RTC_IRQn = 1, /*!< Real Time Clock Interrupt */ - TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */ - TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */ - MCIA_IRQn = 4, /*!< MCIa Interrupt */ - MCIB_IRQn = 5, /*!< MCIb Interrupt */ - UART0_IRQn = 6, /*!< UART0 Interrupt */ - UART1_IRQn = 7, /*!< UART1 Interrupt */ - UART2_IRQn = 8, /*!< UART2 Interrupt */ - UART4_IRQn = 9, /*!< UART4 Interrupt */ - AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */ - CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */ - ENET_IRQn = 12, /*!< Ethernet Interrupt */ - USBDC_IRQn = 13, /*!< USB Device Interrupt */ - USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */ - CHLCD_IRQn = 15, /*!< Character LCD Interrupt */ - FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */ - CAN_IRQn = 17, /*!< CAN Interrupt */ - LIN_IRQn = 18, /*!< LIN Interrupt */ - I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */ - CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */ - UART3_IRQn = 30, /*!< UART3 Interrupt */ - SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */ -} IRQn_Type; - - -/* ================================================================================ */ -/* ================ Processor and Core Peripheral Section ================ */ -/* ================================================================================ */ - -/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */ -#define __CM3_REV 0x0201 /*!< Core revision r2p1 */ -#define __MPU_PRESENT 1 /*!< MPU present or not */ -#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */ -#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */ - -#include /* Processor and core peripherals */ -/* NuttX */ -//#include "system_ARMCM3.h" /* System Header */ - - -/* ================================================================================ */ -/* ================ Device Specific Peripheral Section ================ */ -/* ================================================================================ */ - -/* ------------------- Start of section using anonymous unions ------------------ */ -#if defined(__CC_ARM) - #pragma push - #pragma anon_unions -#elif defined(__ICCARM__) - #pragma language=extended -#elif defined(__GNUC__) - /* anonymous unions are enabled by default */ -#elif defined(__TMS470__) -/* anonymous unions are enabled by default */ -#elif defined(__TASKING__) - #pragma warning 586 -#else - #warning Not supported compiler type -#endif - - - -/* ================================================================================ */ -/* ================ CPU FPGA System (CPU_SYS) ================ */ -/* ================================================================================ */ -typedef struct -{ - __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ - __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */ - __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ - __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ - __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */ - __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */ - uint32_t RESERVED0[2]; - __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */ - __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */ - __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */ - uint32_t RESERVED1[3]; - __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */ - __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */ -} ARM_CPU_SYS_TypeDef; - - -/* ================================================================================ */ -/* ================ DUT FPGA System (DUT_SYS) ================ */ -/* ================================================================================ */ -typedef struct -{ - __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ - __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */ - __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ - __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ - __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */ - __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */ - __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */ -} ARM_DUT_SYS_TypeDef; - - -/* ================================================================================ */ -/* ================ Timer (TIM) ================ */ -/* ================================================================================ */ -typedef struct -{ - __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */ - __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */ - __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */ - __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */ - __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */ - __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */ - __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */ - uint32_t RESERVED0[1]; - __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */ - __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */ - __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */ - __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */ - __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */ - __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */ - __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */ -} ARM_TIM_TypeDef; - - -/* ================================================================================ */ -/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */ -/* ================================================================================ */ -typedef struct -{ - __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */ - union { - __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */ - __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */ - }; - uint32_t RESERVED0[4]; - __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */ - uint32_t RESERVED1[1]; - __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */ - __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */ - __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */ - __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */ - __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */ - __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */ - __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */ - __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */ - __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */ - __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */ - __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */ -} ARM_UART_TypeDef; - - -/* -------------------- End of section using anonymous unions ------------------- */ -#if defined(__CC_ARM) - #pragma pop -#elif defined(__ICCARM__) - /* leave anonymous unions enabled */ -#elif defined(__GNUC__) - /* anonymous unions are enabled by default */ -#elif defined(__TMS470__) - /* anonymous unions are enabled by default */ -#elif defined(__TASKING__) - #pragma warning restore -#else - #warning Not supported compiler type -#endif - - - - -/* ================================================================================ */ -/* ================ Peripheral memory map ================ */ -/* ================================================================================ */ -/* -------------------------- CPU FPGA memory map ------------------------------- */ -#define ARM_FLASH_BASE (0x00000000UL) -#define ARM_RAM_BASE (0x20000000UL) -#define ARM_RAM_FPGA_BASE (0x1EFF0000UL) -#define ARM_CPU_CFG_BASE (0xDFFF0000UL) - -#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000) -#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000) - -/* -------------------------- DUT FPGA memory map ------------------------------- */ -#define ARM_APB_BASE (0x40000000UL) -#define ARM_AHB_BASE (0x4FF00000UL) -#define ARM_DMC_BASE (0x60000000UL) -#define ARM_SMC_BASE (0xA0000000UL) - -#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000) -#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000) -#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000) -#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000) -#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000) -#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000) -#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000) - - -/* ================================================================================ */ -/* ================ Peripheral declaration ================ */ -/* ================================================================================ */ -/* -------------------------- CPU FPGA Peripherals ------------------------------ */ -#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE) -#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE) - -/* -------------------------- DUT FPGA Peripherals ------------------------------ */ -#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE) -#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE) -#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE) -#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE) -#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE) -#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE) -#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE) - - -#ifdef __cplusplus -} -#endif - -#endif /* ARMCM3_H */ diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h deleted file mode 100644 index 181b7e433..000000000 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************//** - * @file ARMCM4.h - * @brief CMSIS Core Peripheral Access Layer Header File for - * ARMCM4 Device Series - * @version V1.07 - * @date 30. January 2012 - * - * @note - * Copyright (C) 2012 ARM Limited. All rights reserved. - * - * @par - * ARM Limited (ARM) is supplying this software for use with Cortex-M - * processor based microcontrollers. This file can be freely distributed - * within development tools that are supporting such ARM based processors. - * - * @par - * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED - * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. - * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR - * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. - * - ******************************************************************************/ - -#ifndef ARMCM4_H -#define ARMCM4_H - -#ifdef __cplusplus -extern "C" { -#endif - - -/* ------------------------- Interrupt Number Definition ------------------------ */ - -typedef enum IRQn -{ -/* ------------------- Cortex-M4 Processor Exceptions Numbers ------------------- */ - NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */ - HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */ - MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */ - BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */ - UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */ - SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */ - DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */ - PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */ - SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */ - -/* ---------------------- ARMCM4 Specific Interrupt Numbers --------------------- */ - WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */ - RTC_IRQn = 1, /*!< Real Time Clock Interrupt */ - TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */ - TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */ - MCIA_IRQn = 4, /*!< MCIa Interrupt */ - MCIB_IRQn = 5, /*!< MCIb Interrupt */ - UART0_IRQn = 6, /*!< UART0 Interrupt */ - UART1_IRQn = 7, /*!< UART1 Interrupt */ - UART2_IRQn = 8, /*!< UART2 Interrupt */ - UART4_IRQn = 9, /*!< UART4 Interrupt */ - AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */ - CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */ - ENET_IRQn = 12, /*!< Ethernet Interrupt */ - USBDC_IRQn = 13, /*!< USB Device Interrupt */ - USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */ - CHLCD_IRQn = 15, /*!< Character LCD Interrupt */ - FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */ - CAN_IRQn = 17, /*!< CAN Interrupt */ - LIN_IRQn = 18, /*!< LIN Interrupt */ - I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */ - CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */ - UART3_IRQn = 30, /*!< UART3 Interrupt */ - SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */ -} IRQn_Type; - - -/* ================================================================================ */ -/* ================ Processor and Core Peripheral Section ================ */ -/* ================================================================================ */ - -/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */ -#define __CM4_REV 0x0001 /*!< Core revision r0p1 */ -#define __MPU_PRESENT 1 /*!< MPU present or not */ -#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */ -#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */ -#define __FPU_PRESENT 1 /*!< FPU present or not */ - -#include /* Processor and core peripherals */ -/* NuttX */ -//#include "system_ARMCM4.h" /* System Header */ - - -/* ================================================================================ */ -/* ================ Device Specific Peripheral Section ================ */ -/* ================================================================================ */ - -/* ------------------- Start of section using anonymous unions ------------------ */ -#if defined(__CC_ARM) - #pragma push - #pragma anon_unions -#elif defined(__ICCARM__) - #pragma language=extended -#elif defined(__GNUC__) - /* anonymous unions are enabled by default */ -#elif defined(__TMS470__) -/* anonymous unions are enabled by default */ -#elif defined(__TASKING__) - #pragma warning 586 -#else - #warning Not supported compiler type -#endif - - - -/* ================================================================================ */ -/* ================ CPU FPGA System (CPU_SYS) ================ */ -/* ================================================================================ */ -typedef struct -{ - __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ - __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */ - __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ - __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ - __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */ - __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */ - uint32_t RESERVED0[2]; - __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */ - __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */ - __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */ - uint32_t RESERVED1[3]; - __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */ - __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */ -} ARM_CPU_SYS_TypeDef; - - -/* ================================================================================ */ -/* ================ DUT FPGA System (DUT_SYS) ================ */ -/* ================================================================================ */ -typedef struct -{ - __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */ - __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */ - __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */ - __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */ - __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */ - __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */ - __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */ -} ARM_DUT_SYS_TypeDef; - - -/* ================================================================================ */ -/* ================ Timer (TIM) ================ */ -/* ================================================================================ */ -typedef struct -{ - __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */ - __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */ - __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */ - __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */ - __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */ - __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */ - __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */ - uint32_t RESERVED0[1]; - __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */ - __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */ - __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */ - __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */ - __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */ - __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */ - __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */ -} ARM_TIM_TypeDef; - - -/* ================================================================================ */ -/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */ -/* ================================================================================ */ -typedef struct -{ - __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */ - union { - __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */ - __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */ - }; - uint32_t RESERVED0[4]; - __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */ - uint32_t RESERVED1[1]; - __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */ - __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */ - __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */ - __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */ - __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */ - __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */ - __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */ - __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */ - __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */ - __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */ - __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */ -} ARM_UART_TypeDef; - - -/* -------------------- End of section using anonymous unions ------------------- */ -#if defined(__CC_ARM) - #pragma pop -#elif defined(__ICCARM__) - /* leave anonymous unions enabled */ -#elif defined(__GNUC__) - /* anonymous unions are enabled by default */ -#elif defined(__TMS470__) - /* anonymous unions are enabled by default */ -#elif defined(__TASKING__) - #pragma warning restore -#else - #warning Not supported compiler type -#endif - - - - -/* ================================================================================ */ -/* ================ Peripheral memory map ================ */ -/* ================================================================================ */ -/* -------------------------- CPU FPGA memory map ------------------------------- */ -#define ARM_FLASH_BASE (0x00000000UL) -#define ARM_RAM_BASE (0x20000000UL) -#define ARM_RAM_FPGA_BASE (0x1EFF0000UL) -#define ARM_CPU_CFG_BASE (0xDFFF0000UL) - -#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000) -#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000) - -/* -------------------------- DUT FPGA memory map ------------------------------- */ -#define ARM_APB_BASE (0x40000000UL) -#define ARM_AHB_BASE (0x4FF00000UL) -#define ARM_DMC_BASE (0x60000000UL) -#define ARM_SMC_BASE (0xA0000000UL) - -#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000) -#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000) -#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000) -#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000) -#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000) -#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000) -#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000) - - -/* ================================================================================ */ -/* ================ Peripheral declaration ================ */ -/* ================================================================================ */ -/* -------------------------- CPU FPGA Peripherals ------------------------------ */ -#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE) -#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE) - -/* -------------------------- DUT FPGA Peripherals ------------------------------ */ -#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE) -#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE) -#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE) -#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE) -#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE) -#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE) -#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE) - - -#ifdef __cplusplus -} -#endif - -#endif /* ARMCM4_H */ diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h deleted file mode 100644 index 9c37ab4e5..000000000 --- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h +++ /dev/null @@ -1,93 +0,0 @@ -/* ---------------------------------------------------------------------- -* Copyright (C) 2010-2013 ARM Limited. All rights reserved. -* -* $Date: 17. January 2013 -* $Revision: V1.4.1 -* -* Project: CMSIS DSP Library -* Title: arm_common_tables.h -* -* Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions -* -* Target Processor: Cortex-M4/Cortex-M3 -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* - Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* - 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. -* - Neither the name of ARM LIMITED 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 _ARM_COMMON_TABLES_H -#define _ARM_COMMON_TABLES_H - -#include "arm_math.h" - -extern const uint16_t armBitRevTable[1024]; -extern const q15_t armRecipTableQ15[64]; -extern const q31_t armRecipTableQ31[64]; -extern const q31_t realCoefAQ31[1024]; -extern const q31_t realCoefBQ31[1024]; -extern const float32_t twiddleCoef_16[32]; -extern const float32_t twiddleCoef_32[64]; -extern const float32_t twiddleCoef_64[128]; -extern const float32_t twiddleCoef_128[256]; -extern const float32_t twiddleCoef_256[512]; -extern const float32_t twiddleCoef_512[1024]; -extern const float32_t twiddleCoef_1024[2048]; -extern const float32_t twiddleCoef_2048[4096]; -extern const float32_t twiddleCoef_4096[8192]; -#define twiddleCoef twiddleCoef_4096 -extern const q31_t twiddleCoefQ31[6144]; -extern const q15_t twiddleCoefQ15[6144]; -extern const float32_t twiddleCoef_rfft_32[32]; -extern const float32_t twiddleCoef_rfft_64[64]; -extern const float32_t twiddleCoef_rfft_128[128]; -extern const float32_t twiddleCoef_rfft_256[256]; -extern const float32_t twiddleCoef_rfft_512[512]; -extern const float32_t twiddleCoef_rfft_1024[1024]; -extern const float32_t twiddleCoef_rfft_2048[2048]; -extern const float32_t twiddleCoef_rfft_4096[4096]; - - -#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 ) -#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 ) -#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 ) -#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 ) -#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 ) -#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 ) -#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800) -#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808) -#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032) - -extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH]; -extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH]; - -#endif /* ARM_COMMON_TABLES_H */ diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/modules/mathlib/CMSIS/Include/arm_const_structs.h deleted file mode 100644 index 406f737dc..000000000 --- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h +++ /dev/null @@ -1,85 +0,0 @@ -/* ---------------------------------------------------------------------- -* Copyright (C) 2010-2013 ARM Limited. All rights reserved. -* -* $Date: 17. January 2013 -* $Revision: V1.4.1 -* -* Project: CMSIS DSP Library -* Title: arm_const_structs.h -* -* Description: This file has constant structs that are initialized for -* user convenience. For example, some can be given as -* arguments to the arm_cfft_f32() function. -* -* Target Processor: Cortex-M4/Cortex-M3 -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* - Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* - 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. -* - Neither the name of ARM LIMITED 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 _ARM_CONST_STRUCTS_H -#define _ARM_CONST_STRUCTS_H - -#include "arm_math.h" -#include "arm_common_tables.h" - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = { - 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = { - 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = { - 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = { - 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = { - 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = { - 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = { - 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = { - 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH - }; - - const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = { - 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH - }; - -#endif diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/modules/mathlib/CMSIS/Include/arm_math.h deleted file mode 100644 index 6f66f9ee3..000000000 --- a/src/modules/mathlib/CMSIS/Include/arm_math.h +++ /dev/null @@ -1,7318 +0,0 @@ -/* ---------------------------------------------------------------------- -* Copyright (C) 2010-2013 ARM Limited. All rights reserved. -* -* $Date: 17. January 2013 -* $Revision: V1.4.1 -* -* Project: CMSIS DSP Library -* Title: arm_math.h -* -* Description: Public header file for CMSIS DSP Library -* -* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* - Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* - 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. -* - Neither the name of ARM LIMITED 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. - * -------------------------------------------------------------------- */ - -/** - \mainpage CMSIS DSP Software Library - * - * Introduction - * - * This user manual describes the CMSIS DSP software library, - * a suite of common signal processing functions for use on Cortex-M processor based devices. - * - * The library is divided into a number of functions each covering a specific category: - * - Basic math functions - * - Fast math functions - * - Complex math functions - * - Filters - * - Matrix functions - * - Transforms - * - Motor control functions - * - Statistical functions - * - Support functions - * - Interpolation functions - * - * The library has separate functions for operating on 8-bit integers, 16-bit integers, - * 32-bit integer and 32-bit floating-point values. - * - * Using the Library - * - * The library installer contains prebuilt versions of the libraries in the Lib folder. - * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4) - * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4) - * - arm_cortexM4l_math.lib (Little endian on Cortex-M4) - * - arm_cortexM4b_math.lib (Big endian on Cortex-M4) - * - arm_cortexM3l_math.lib (Little endian on Cortex-M3) - * - arm_cortexM3b_math.lib (Big endian on Cortex-M3) - * - arm_cortexM0l_math.lib (Little endian on Cortex-M0) - * - arm_cortexM0b_math.lib (Big endian on Cortex-M3) - * - * The library functions are declared in the public file arm_math.h which is placed in the Include folder. - * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single - * public header file arm_math.h for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants. - * Define the appropriate pre processor MACRO ARM_MATH_CM4 or ARM_MATH_CM3 or - * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application. - * - * Examples - * - * The library ships with a number of examples which demonstrate how to use the library functions. - * - * Toolchain Support - * - * The library has been developed and tested with MDK-ARM version 4.60. - * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly. - * - * Building the Library - * - * The library installer contains project files to re build libraries on MDK Tool chain in the CMSIS\\DSP_Lib\\Source\\ARM folder. - * - arm_cortexM0b_math.uvproj - * - arm_cortexM0l_math.uvproj - * - arm_cortexM3b_math.uvproj - * - arm_cortexM3l_math.uvproj - * - arm_cortexM4b_math.uvproj - * - arm_cortexM4l_math.uvproj - * - arm_cortexM4bf_math.uvproj - * - arm_cortexM4lf_math.uvproj - * - * - * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above. - * - * Pre-processor Macros - * - * Each library project have differant pre-processor macros. - * - * - UNALIGNED_SUPPORT_DISABLE: - * - * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access - * - * - ARM_MATH_BIG_ENDIAN: - * - * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets. - * - * - ARM_MATH_MATRIX_CHECK: - * - * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices - * - * - ARM_MATH_ROUNDING: - * - * Define macro ARM_MATH_ROUNDING for rounding on support functions - * - * - ARM_MATH_CMx: - * - * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target - * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target. - * - * - __FPU_PRESENT: - * - * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries - * - * Copyright Notice - * - * Copyright (C) 2010-2013 ARM Limited. All rights reserved. - */ - - -/** - * @defgroup groupMath Basic Math Functions - */ - -/** - * @defgroup groupFastMath Fast Math Functions - * This set of functions provides a fast approximation to sine, cosine, and square root. - * As compared to most of the other functions in the CMSIS math library, the fast math functions - * operate on individual values and not arrays. - * There are separate functions for Q15, Q31, and floating-point data. - * - */ - -/** - * @defgroup groupCmplxMath Complex Math Functions - * This set of functions operates on complex data vectors. - * The data in the complex arrays is stored in an interleaved fashion - * (real, imag, real, imag, ...). - * In the API functions, the number of samples in a complex array refers - * to the number of complex values; the array contains twice this number of - * real values. - */ - -/** - * @defgroup groupFilters Filtering Functions - */ - -/** - * @defgroup groupMatrix Matrix Functions - * - * This set of functions provides basic matrix math operations. - * The functions operate on matrix data structures. For example, - * the type - * definition for the floating-point matrix structure is shown - * below: - * 
- *     typedef struct
- *     {
- *       uint16_t numRows;     // number of rows of the matrix.
- *       uint16_t numCols;     // number of columns of the matrix.
- *       float32_t *pData;     // points to the data of the matrix.
- *     } arm_matrix_instance_f32;
- *
- * There are similar definitions for Q15 and Q31 data types. - * - * The structure specifies the size of the matrix and then points to - * an array of data. The array is of size numRows X numCols - * and the values are arranged in row order. That is, the - * matrix element (i, j) is stored at: - * 
- *     pData[i*numCols + j]
- *
- * - * \par Init Functions - * There is an associated initialization function for each type of matrix - * data structure. - * The initialization function sets the values of the internal structure fields. - * Refer to the function arm_mat_init_f32(), arm_mat_init_q31() - * and arm_mat_init_q15() for floating-point, Q31 and Q15 types, respectively. - * - * \par - * Use of the initialization function is optional. However, if initialization function is used - * then the instance structure cannot be placed into a const data section. - * To place the instance structure in a const data - * section, manually initialize the data structure. For example: - * 
- * arm_matrix_instance_f32 S = {nRows, nColumns, pData};
- * arm_matrix_instance_q31 S = {nRows, nColumns, pData};
- * arm_matrix_instance_q15 S = {nRows, nColumns, pData};
- *
- * where nRows specifies the number of rows, nColumns - * specifies the number of columns, and pData points to the - * data array. - * - * \par Size Checking - * By default all of the matrix functions perform size checking on the input and - * output matrices. For example, the matrix addition function verifies that the - * two input matrices and the output matrix all have the same number of rows and - * columns. If the size check fails the functions return: - * 
- *     ARM_MATH_SIZE_MISMATCH
- *
- * Otherwise the functions return - * 
- *     ARM_MATH_SUCCESS
- *
- * There is some overhead associated with this matrix size checking. - * The matrix size checking is enabled via the \#define - * 
- *     ARM_MATH_MATRIX_CHECK
- *
- * within the library project settings. By default this macro is defined - * and size checking is enabled. By changing the project settings and - * undefining this macro size checking is eliminated and the functions - * run a bit faster. With size checking disabled the functions always - * return ARM_MATH_SUCCESS. - */ - -/** - * @defgroup groupTransforms Transform Functions - */ - -/** - * @defgroup groupController Controller Functions - */ - -/** - * @defgroup groupStats Statistics Functions - */ -/** - * @defgroup groupSupport Support Functions - */ - -/** - * @defgroup groupInterpolation Interpolation Functions - * These functions perform 1- and 2-dimensional interpolation of data. - * Linear interpolation is used for 1-dimensional data and - * bilinear interpolation is used for 2-dimensional data. - */ - -/** - * @defgroup groupExamples Examples - */ -#ifndef _ARM_MATH_H -#define _ARM_MATH_H - -#define __CMSIS_GENERIC /* disable NVIC and Systick functions */ - -/* PX4 */ -#include -#ifdef CONFIG_ARCH_CORTEXM4 -# define ARM_MATH_CM4 1 -#endif -#ifdef CONFIG_ARCH_CORTEXM3 -# define ARM_MATH_CM3 1 -#endif -#ifdef CONFIG_ARCH_FPU -# define __FPU_PRESENT 1 -#endif - -#if defined (ARM_MATH_CM4) -#include "core_cm4.h" -#elif defined (ARM_MATH_CM3) -#include "core_cm3.h" -#elif defined (ARM_MATH_CM0) -#include "core_cm0.h" -#define ARM_MATH_CM0_FAMILY -#elif defined (ARM_MATH_CM0PLUS) -#include "core_cm0plus.h" -#define ARM_MATH_CM0_FAMILY -#else -#include "ARMCM4.h" -#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....." -#endif - -#undef __CMSIS_GENERIC /* enable NVIC and Systick functions */ -#include "string.h" -#include "math.h" -#ifdef __cplusplus -extern "C" -{ -#endif - - - /** - * @brief Macros required for reciprocal calculation in Normalized LMS - */ - -#define DELTA_Q31 (0x100) -#define DELTA_Q15 0x5 -#define INDEX_MASK 0x0000003F -#ifndef PI -#define PI 3.14159265358979f -#endif - - /** - * @brief Macros required for SINE and COSINE Fast math approximations - */ - -#define TABLE_SIZE 256 -#define TABLE_SPACING_Q31 0x800000 -#define TABLE_SPACING_Q15 0x80 - - /** - * @brief Macros required for SINE and COSINE Controller functions - */ - /* 1.31(q31) Fixed value of 2/360 */ - /* -1 to +1 is divided into 360 values so total spacing is (2/360) */ -#define INPUT_SPACING 0xB60B61 - - /** - * @brief Macro for Unaligned Support - */ -#ifndef UNALIGNED_SUPPORT_DISABLE - #define ALIGN4 -#else - #if defined (__GNUC__) - #define ALIGN4 __attribute__((aligned(4))) - #else - #define ALIGN4 __align(4) - #endif -#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ - - /** - * @brief Error status returned by some functions in the library. - */ - - typedef enum - { - ARM_MATH_SUCCESS = 0, /**< No error */ - ARM_MATH_ARGUMENT_ERROR = -1, /**< One or more arguments are incorrect */ - ARM_MATH_LENGTH_ERROR = -2, /**< Length of data buffer is incorrect */ - ARM_MATH_SIZE_MISMATCH = -3, /**< Size of matrices is not compatible with the operation. */ - ARM_MATH_NANINF = -4, /**< Not-a-number (NaN) or infinity is generated */ - ARM_MATH_SINGULAR = -5, /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */ - ARM_MATH_TEST_FAILURE = -6 /**< Test Failed */ - } arm_status; - - /** - * @brief 8-bit fractional data type in 1.7 format. - */ - typedef int8_t q7_t; - - /** - * @brief 16-bit fractional data type in 1.15 format. - */ - typedef int16_t q15_t; - - /** - * @brief 32-bit fractional data type in 1.31 format. - */ - typedef int32_t q31_t; - - /** - * @brief 64-bit fractional data type in 1.63 format. - */ - typedef int64_t q63_t; - - /** - * @brief 32-bit floating-point type definition. - */ - typedef float float32_t; - - /** - * @brief 64-bit floating-point type definition. - */ - typedef double float64_t; - - /** - * @brief definition to read/write two 16 bit values. - */ -#if defined __CC_ARM -#define __SIMD32_TYPE int32_t __packed -#define CMSIS_UNUSED __attribute__((unused)) -#elif defined __ICCARM__ -#define CMSIS_UNUSED -#define __SIMD32_TYPE int32_t __packed -#elif defined __GNUC__ -#define __SIMD32_TYPE int32_t -#define CMSIS_UNUSED __attribute__((unused)) -#else -#error Unknown compiler -#endif - -#define __SIMD32(addr) (*(__SIMD32_TYPE **) & (addr)) -#define __SIMD32_CONST(addr) ((__SIMD32_TYPE *)(addr)) - -#define _SIMD32_OFFSET(addr) (*(__SIMD32_TYPE *) (addr)) - -#define __SIMD64(addr) (*(int64_t **) & (addr)) - -#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) - /** - * @brief definition to pack two 16 bit values. - */ -#define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0x0000FFFF) | \ - (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000) ) -#define __PKHTB(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0xFFFF0000) | \ - (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF) ) - -#endif - - - /** - * @brief definition to pack four 8 bit values. - */ -#ifndef ARM_MATH_BIG_ENDIAN - -#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) << 0) & (int32_t)0x000000FF) | \ - (((int32_t)(v1) << 8) & (int32_t)0x0000FF00) | \ - (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \ - (((int32_t)(v3) << 24) & (int32_t)0xFF000000) ) -#else - -#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) << 0) & (int32_t)0x000000FF) | \ - (((int32_t)(v2) << 8) & (int32_t)0x0000FF00) | \ - (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \ - (((int32_t)(v0) << 24) & (int32_t)0xFF000000) ) - -#endif - - - /** - * @brief Clips Q63 to Q31 values. - */ - static __INLINE q31_t clip_q63_to_q31( - q63_t x) - { - return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ? - ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x; - } - - /** - * @brief Clips Q63 to Q15 values. - */ - static __INLINE q15_t clip_q63_to_q15( - q63_t x) - { - return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ? - ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15); - } - - /** - * @brief Clips Q31 to Q7 values. - */ - static __INLINE q7_t clip_q31_to_q7( - q31_t x) - { - return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ? - ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x; - } - - /** - * @brief Clips Q31 to Q15 values. - */ - static __INLINE q15_t clip_q31_to_q15( - q31_t x) - { - return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ? - ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x; - } - - /** - * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format. - */ - - static __INLINE q63_t mult32x64( - q63_t x, - q31_t y) - { - return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) + - (((q63_t) (x >> 32) * y))); - } - - -#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM ) -#define __CLZ __clz -#endif - -#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) ) - - static __INLINE uint32_t __CLZ( - q31_t data); - - - static __INLINE uint32_t __CLZ( - q31_t data) - { - uint32_t count = 0; - uint32_t mask = 0x80000000; - - while((data & mask) == 0) - { - count += 1u; - mask = mask >> 1u; - } - - return (count); - - } - -#endif - - /** - * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type. - */ - - static __INLINE uint32_t arm_recip_q31( - q31_t in, - q31_t * dst, - q31_t * pRecipTable) - { - - uint32_t out, tempVal; - uint32_t index, i; - uint32_t signBits; - - if(in > 0) - { - signBits = __CLZ(in) - 1; - } - else - { - signBits = __CLZ(-in) - 1; - } - - /* Convert input sample to 1.31 format */ - in = in << signBits; - - /* calculation of index for initial approximated Val */ - index = (uint32_t) (in >> 24u); - index = (index & INDEX_MASK); - - /* 1.31 with exp 1 */ - out = pRecipTable[index]; - - /* calculation of reciprocal value */ - /* running approximation for two iterations */ - for (i = 0u; i < 2u; i++) - { - tempVal = (q31_t) (((q63_t) in * out) >> 31u); - tempVal = 0x7FFFFFFF - tempVal; - /* 1.31 with exp 1 */ - //out = (q31_t) (((q63_t) out * tempVal) >> 30u); - out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u); - } - - /* write output */ - *dst = out; - - /* return num of signbits of out = 1/in value */ - return (signBits + 1u); - - } - - /** - * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type. - */ - static __INLINE uint32_t arm_recip_q15( - q15_t in, - q15_t * dst, - q15_t * pRecipTable) - { - - uint32_t out = 0, tempVal = 0; - uint32_t index = 0, i = 0; - uint32_t signBits = 0; - - if(in > 0) - { - signBits = __CLZ(in) - 17; - } - else - { - signBits = __CLZ(-in) - 17; - } - - /* Convert input sample to 1.15 format */ - in = in << signBits; - - /* calculation of index for initial approximated Val */ - index = in >> 8; - index = (index & INDEX_MASK); - - /* 1.15 with exp 1 */ - out = pRecipTable[index]; - - /* calculation of reciprocal value */ - /* running approximation for two iterations */ - for (i = 0; i < 2; i++) - { - tempVal = (q15_t) (((q31_t) in * out) >> 15); - tempVal = 0x7FFF - tempVal; - /* 1.15 with exp 1 */ - out = (q15_t) (((q31_t) out * tempVal) >> 14); - } - - /* write output */ - *dst = out; - - /* return num of signbits of out = 1/in value */ - return (signBits + 1); - - } - - - /* - * @brief C custom defined intrinisic function for only M0 processors - */ -#if defined(ARM_MATH_CM0_FAMILY) - - static __INLINE q31_t __SSAT( - q31_t x, - uint32_t y) - { - int32_t posMax, negMin; - uint32_t i; - - posMax = 1; - for (i = 0; i < (y - 1); i++) - { - posMax = posMax * 2; - } - - if(x > 0) - { - posMax = (posMax - 1); - - if(x > posMax) - { - x = posMax; - } - } - else - { - negMin = -posMax; - - if(x < negMin) - { - x = negMin; - } - } - return (x); - - - } - -#endif /* end of ARM_MATH_CM0_FAMILY */ - - - - /* - * @brief C custom defined intrinsic function for M3 and M0 processors - */ -#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) - - /* - * @brief C custom defined QADD8 for M3 and M0 processors - */ - static __INLINE q31_t __QADD8( - q31_t x, - q31_t y) - { - - q31_t sum; - q7_t r, s, t, u; - - r = (q7_t) x; - s = (q7_t) y; - - r = __SSAT((q31_t) (r + s), 8); - s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8); - t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8); - u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8); - - sum = - (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) | - (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF); - - return sum; - - } - - /* - * @brief C custom defined QSUB8 for M3 and M0 processors - */ - static __INLINE q31_t __QSUB8( - q31_t x, - q31_t y) - { - - q31_t sum; - q31_t r, s, t, u; - - r = (q7_t) x; - s = (q7_t) y; - - r = __SSAT((r - s), 8); - s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8; - t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16; - u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24; - - sum = - (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r & - 0x000000FF); - - return sum; - } - - /* - * @brief C custom defined QADD16 for M3 and M0 processors - */ - - /* - * @brief C custom defined QADD16 for M3 and M0 processors - */ - static __INLINE q31_t __QADD16( - q31_t x, - q31_t y) - { - - q31_t sum; - q31_t r, s; - - r = (short) x; - s = (short) y; - - r = __SSAT(r + s, 16); - s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16; - - sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); - - return sum; - - } - - /* - * @brief C custom defined SHADD16 for M3 and M0 processors - */ - static __INLINE q31_t __SHADD16( - q31_t x, - q31_t y) - { - - q31_t sum; - q31_t r, s; - - r = (short) x; - s = (short) y; - - r = ((r >> 1) + (s >> 1)); - s = ((q31_t) ((x >> 17) + (y >> 17))) << 16; - - sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); - - return sum; - - } - - /* - * @brief C custom defined QSUB16 for M3 and M0 processors - */ - static __INLINE q31_t __QSUB16( - q31_t x, - q31_t y) - { - - q31_t sum; - q31_t r, s; - - r = (short) x; - s = (short) y; - - r = __SSAT(r - s, 16); - s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16; - - sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); - - return sum; - } - - /* - * @brief C custom defined SHSUB16 for M3 and M0 processors - */ - static __INLINE q31_t __SHSUB16( - q31_t x, - q31_t y) - { - - q31_t diff; - q31_t r, s; - - r = (short) x; - s = (short) y; - - r = ((r >> 1) - (s >> 1)); - s = (((x >> 17) - (y >> 17)) << 16); - - diff = (s & 0xFFFF0000) | (r & 0x0000FFFF); - - return diff; - } - - /* - * @brief C custom defined QASX for M3 and M0 processors - */ - static __INLINE q31_t __QASX( - q31_t x, - q31_t y) - { - - q31_t sum = 0; - - sum = - ((sum + - clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) + - clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16))); - - return sum; - } - - /* - * @brief C custom defined SHASX for M3 and M0 processors - */ - static __INLINE q31_t __SHASX( - q31_t x, - q31_t y) - { - - q31_t sum; - q31_t r, s; - - r = (short) x; - s = (short) y; - - r = ((r >> 1) - (y >> 17)); - s = (((x >> 17) + (s >> 1)) << 16); - - sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); - - return sum; - } - - - /* - * @brief C custom defined QSAX for M3 and M0 processors - */ - static __INLINE q31_t __QSAX( - q31_t x, - q31_t y) - { - - q31_t sum = 0; - - sum = - ((sum + - clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) + - clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16))); - - return sum; - } - - /* - * @brief C custom defined SHSAX for M3 and M0 processors - */ - static __INLINE q31_t __SHSAX( - q31_t x, - q31_t y) - { - - q31_t sum; - q31_t r, s; - - r = (short) x; - s = (short) y; - - r = ((r >> 1) + (y >> 17)); - s = (((x >> 17) - (s >> 1)) << 16); - - sum = (s & 0xFFFF0000) | (r & 0x0000FFFF); - - return sum; - } - - /* - * @brief C custom defined SMUSDX for M3 and M0 processors - */ - static __INLINE q31_t __SMUSDX( - q31_t x, - q31_t y) - { - - return ((q31_t) (((short) x * (short) (y >> 16)) - - ((short) (x >> 16) * (short) y))); - } - - /* - * @brief C custom defined SMUADX for M3 and M0 processors - */ - static __INLINE q31_t __SMUADX( - q31_t x, - q31_t y) - { - - return ((q31_t) (((short) x * (short) (y >> 16)) + - ((short) (x >> 16) * (short) y))); - } - - /* - * @brief C custom defined QADD for M3 and M0 processors - */ - static __INLINE q31_t __QADD( - q31_t x, - q31_t y) - { - return clip_q63_to_q31((q63_t) x + y); - } - - /* - * @brief C custom defined QSUB for M3 and M0 processors - */ - static __INLINE q31_t __QSUB( - q31_t x, - q31_t y) - { - return clip_q63_to_q31((q63_t) x - y); - } - - /* - * @brief C custom defined SMLAD for M3 and M0 processors - */ - static __INLINE q31_t __SMLAD( - q31_t x, - q31_t y, - q31_t sum) - { - - return (sum + ((short) (x >> 16) * (short) (y >> 16)) + - ((short) x * (short) y)); - } - - /* - * @brief C custom defined SMLADX for M3 and M0 processors - */ - static __INLINE q31_t __SMLADX( - q31_t x, - q31_t y, - q31_t sum) - { - - return (sum + ((short) (x >> 16) * (short) (y)) + - ((short) x * (short) (y >> 16))); - } - - /* - * @brief C custom defined SMLSDX for M3 and M0 processors - */ - static __INLINE q31_t __SMLSDX( - q31_t x, - q31_t y, - q31_t sum) - { - - return (sum - ((short) (x >> 16) * (short) (y)) + - ((short) x * (short) (y >> 16))); - } - - /* - * @brief C custom defined SMLALD for M3 and M0 processors - */ - static __INLINE q63_t __SMLALD( - q31_t x, - q31_t y, - q63_t sum) - { - - return (sum + ((short) (x >> 16) * (short) (y >> 16)) + - ((short) x * (short) y)); - } - - /* - * @brief C custom defined SMLALDX for M3 and M0 processors - */ - static __INLINE q63_t __SMLALDX( - q31_t x, - q31_t y, - q63_t sum) - { - - return (sum + ((short) (x >> 16) * (short) y)) + - ((short) x * (short) (y >> 16)); - } - - /* - * @brief C custom defined SMUAD for M3 and M0 processors - */ - static __INLINE q31_t __SMUAD( - q31_t x, - q31_t y) - { - - return (((x >> 16) * (y >> 16)) + - (((x << 16) >> 16) * ((y << 16) >> 16))); - } - - /* - * @brief C custom defined SMUSD for M3 and M0 processors - */ - static __INLINE q31_t __SMUSD( - q31_t x, - q31_t y) - { - - return (-((x >> 16) * (y >> 16)) + - (((x << 16) >> 16) * ((y << 16) >> 16))); - } - - - /* - * @brief C custom defined SXTB16 for M3 and M0 processors - */ - static __INLINE q31_t __SXTB16( - q31_t x) - { - - return ((((x << 24) >> 24) & 0x0000FFFF) | - (((x << 8) >> 8) & 0xFFFF0000)); - } - - -#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */ - - - /** - * @brief Instance structure for the Q7 FIR filter. - */ - typedef struct - { - uint16_t numTaps; /**< number of filter coefficients in the filter. */ - q7_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - } arm_fir_instance_q7; - - /** - * @brief Instance structure for the Q15 FIR filter. - */ - typedef struct - { - uint16_t numTaps; /**< number of filter coefficients in the filter. */ - q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - } arm_fir_instance_q15; - - /** - * @brief Instance structure for the Q31 FIR filter. - */ - typedef struct - { - uint16_t numTaps; /**< number of filter coefficients in the filter. */ - q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - } arm_fir_instance_q31; - - /** - * @brief Instance structure for the floating-point FIR filter. - */ - typedef struct - { - uint16_t numTaps; /**< number of filter coefficients in the filter. */ - float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - } arm_fir_instance_f32; - - - /** - * @brief Processing function for the Q7 FIR filter. - * @param[in] *S points to an instance of the Q7 FIR filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_q7( - const arm_fir_instance_q7 * S, - q7_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - - /** - * @brief Initialization function for the Q7 FIR filter. - * @param[in,out] *S points to an instance of the Q7 FIR structure. - * @param[in] numTaps Number of filter coefficients in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of samples that are processed. - * @return none - */ - void arm_fir_init_q7( - arm_fir_instance_q7 * S, - uint16_t numTaps, - q7_t * pCoeffs, - q7_t * pState, - uint32_t blockSize); - - - /** - * @brief Processing function for the Q15 FIR filter. - * @param[in] *S points to an instance of the Q15 FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_q15( - const arm_fir_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4. - * @param[in] *S points to an instance of the Q15 FIR filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_fast_q15( - const arm_fir_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q15 FIR filter. - * @param[in,out] *S points to an instance of the Q15 FIR filter structure. - * @param[in] numTaps Number of filter coefficients in the filter. Must be even and greater than or equal to 4. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of samples that are processed at a time. - * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if - * numTaps is not a supported value. - */ - - arm_status arm_fir_init_q15( - arm_fir_instance_q15 * S, - uint16_t numTaps, - q15_t * pCoeffs, - q15_t * pState, - uint32_t blockSize); - - /** - * @brief Processing function for the Q31 FIR filter. - * @param[in] *S points to an instance of the Q31 FIR filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_q31( - const arm_fir_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4. - * @param[in] *S points to an instance of the Q31 FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_fast_q31( - const arm_fir_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q31 FIR filter. - * @param[in,out] *S points to an instance of the Q31 FIR structure. - * @param[in] numTaps Number of filter coefficients in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of samples that are processed at a time. - * @return none. - */ - void arm_fir_init_q31( - arm_fir_instance_q31 * S, - uint16_t numTaps, - q31_t * pCoeffs, - q31_t * pState, - uint32_t blockSize); - - /** - * @brief Processing function for the floating-point FIR filter. - * @param[in] *S points to an instance of the floating-point FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_f32( - const arm_fir_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the floating-point FIR filter. - * @param[in,out] *S points to an instance of the floating-point FIR filter structure. - * @param[in] numTaps Number of filter coefficients in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of samples that are processed at a time. - * @return none. - */ - void arm_fir_init_f32( - arm_fir_instance_f32 * S, - uint16_t numTaps, - float32_t * pCoeffs, - float32_t * pState, - uint32_t blockSize); - - - /** - * @brief Instance structure for the Q15 Biquad cascade filter. - */ - typedef struct - { - int8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ - q15_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ - q15_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ - int8_t postShift; /**< Additional shift, in bits, applied to each output sample. */ - - } arm_biquad_casd_df1_inst_q15; - - - /** - * @brief Instance structure for the Q31 Biquad cascade filter. - */ - typedef struct - { - uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ - q31_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ - q31_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ - uint8_t postShift; /**< Additional shift, in bits, applied to each output sample. */ - - } arm_biquad_casd_df1_inst_q31; - - /** - * @brief Instance structure for the floating-point Biquad cascade filter. - */ - typedef struct - { - uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ - float32_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ - float32_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ - - - } arm_biquad_casd_df1_inst_f32; - - - - /** - * @brief Processing function for the Q15 Biquad cascade filter. - * @param[in] *S points to an instance of the Q15 Biquad cascade structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cascade_df1_q15( - const arm_biquad_casd_df1_inst_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q15 Biquad cascade filter. - * @param[in,out] *S points to an instance of the Q15 Biquad cascade structure. - * @param[in] numStages number of 2nd order stages in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format - * @return none - */ - - void arm_biquad_cascade_df1_init_q15( - arm_biquad_casd_df1_inst_q15 * S, - uint8_t numStages, - q15_t * pCoeffs, - q15_t * pState, - int8_t postShift); - - - /** - * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4. - * @param[in] *S points to an instance of the Q15 Biquad cascade structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cascade_df1_fast_q15( - const arm_biquad_casd_df1_inst_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - - /** - * @brief Processing function for the Q31 Biquad cascade filter - * @param[in] *S points to an instance of the Q31 Biquad cascade structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cascade_df1_q31( - const arm_biquad_casd_df1_inst_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4. - * @param[in] *S points to an instance of the Q31 Biquad cascade structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cascade_df1_fast_q31( - const arm_biquad_casd_df1_inst_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q31 Biquad cascade filter. - * @param[in,out] *S points to an instance of the Q31 Biquad cascade structure. - * @param[in] numStages number of 2nd order stages in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format - * @return none - */ - - void arm_biquad_cascade_df1_init_q31( - arm_biquad_casd_df1_inst_q31 * S, - uint8_t numStages, - q31_t * pCoeffs, - q31_t * pState, - int8_t postShift); - - /** - * @brief Processing function for the floating-point Biquad cascade filter. - * @param[in] *S points to an instance of the floating-point Biquad cascade structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cascade_df1_f32( - const arm_biquad_casd_df1_inst_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the floating-point Biquad cascade filter. - * @param[in,out] *S points to an instance of the floating-point Biquad cascade structure. - * @param[in] numStages number of 2nd order stages in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @return none - */ - - void arm_biquad_cascade_df1_init_f32( - arm_biquad_casd_df1_inst_f32 * S, - uint8_t numStages, - float32_t * pCoeffs, - float32_t * pState); - - - /** - * @brief Instance structure for the floating-point matrix structure. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows of the matrix. */ - uint16_t numCols; /**< number of columns of the matrix. */ - float32_t *pData; /**< points to the data of the matrix. */ - } arm_matrix_instance_f32; - - /** - * @brief Instance structure for the Q15 matrix structure. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows of the matrix. */ - uint16_t numCols; /**< number of columns of the matrix. */ - q15_t *pData; /**< points to the data of the matrix. */ - - } arm_matrix_instance_q15; - - /** - * @brief Instance structure for the Q31 matrix structure. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows of the matrix. */ - uint16_t numCols; /**< number of columns of the matrix. */ - q31_t *pData; /**< points to the data of the matrix. */ - - } arm_matrix_instance_q31; - - - - /** - * @brief Floating-point matrix addition. - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_add_f32( - const arm_matrix_instance_f32 * pSrcA, - const arm_matrix_instance_f32 * pSrcB, - arm_matrix_instance_f32 * pDst); - - /** - * @brief Q15 matrix addition. - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_add_q15( - const arm_matrix_instance_q15 * pSrcA, - const arm_matrix_instance_q15 * pSrcB, - arm_matrix_instance_q15 * pDst); - - /** - * @brief Q31 matrix addition. - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_add_q31( - const arm_matrix_instance_q31 * pSrcA, - const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst); - - - /** - * @brief Floating-point matrix transpose. - * @param[in] *pSrc points to the input matrix - * @param[out] *pDst points to the output matrix - * @return The function returns either ARM_MATH_SIZE_MISMATCH - * or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_trans_f32( - const arm_matrix_instance_f32 * pSrc, - arm_matrix_instance_f32 * pDst); - - - /** - * @brief Q15 matrix transpose. - * @param[in] *pSrc points to the input matrix - * @param[out] *pDst points to the output matrix - * @return The function returns either ARM_MATH_SIZE_MISMATCH - * or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_trans_q15( - const arm_matrix_instance_q15 * pSrc, - arm_matrix_instance_q15 * pDst); - - /** - * @brief Q31 matrix transpose. - * @param[in] *pSrc points to the input matrix - * @param[out] *pDst points to the output matrix - * @return The function returns either ARM_MATH_SIZE_MISMATCH - * or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_trans_q31( - const arm_matrix_instance_q31 * pSrc, - arm_matrix_instance_q31 * pDst); - - - /** - * @brief Floating-point matrix multiplication - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_mult_f32( - const arm_matrix_instance_f32 * pSrcA, - const arm_matrix_instance_f32 * pSrcB, - arm_matrix_instance_f32 * pDst); - - /** - * @brief Q15 matrix multiplication - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @param[in] *pState points to the array for storing intermediate results - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_mult_q15( - const arm_matrix_instance_q15 * pSrcA, - const arm_matrix_instance_q15 * pSrcB, - arm_matrix_instance_q15 * pDst, - q15_t * pState); - - /** - * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @param[in] *pState points to the array for storing intermediate results - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_mult_fast_q15( - const arm_matrix_instance_q15 * pSrcA, - const arm_matrix_instance_q15 * pSrcB, - arm_matrix_instance_q15 * pDst, - q15_t * pState); - - /** - * @brief Q31 matrix multiplication - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_mult_q31( - const arm_matrix_instance_q31 * pSrcA, - const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst); - - /** - * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_mult_fast_q31( - const arm_matrix_instance_q31 * pSrcA, - const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst); - - - /** - * @brief Floating-point matrix subtraction - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_sub_f32( - const arm_matrix_instance_f32 * pSrcA, - const arm_matrix_instance_f32 * pSrcB, - arm_matrix_instance_f32 * pDst); - - /** - * @brief Q15 matrix subtraction - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_sub_q15( - const arm_matrix_instance_q15 * pSrcA, - const arm_matrix_instance_q15 * pSrcB, - arm_matrix_instance_q15 * pDst); - - /** - * @brief Q31 matrix subtraction - * @param[in] *pSrcA points to the first input matrix structure - * @param[in] *pSrcB points to the second input matrix structure - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_sub_q31( - const arm_matrix_instance_q31 * pSrcA, - const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst); - - /** - * @brief Floating-point matrix scaling. - * @param[in] *pSrc points to the input matrix - * @param[in] scale scale factor - * @param[out] *pDst points to the output matrix - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_scale_f32( - const arm_matrix_instance_f32 * pSrc, - float32_t scale, - arm_matrix_instance_f32 * pDst); - - /** - * @brief Q15 matrix scaling. - * @param[in] *pSrc points to input matrix - * @param[in] scaleFract fractional portion of the scale factor - * @param[in] shift number of bits to shift the result by - * @param[out] *pDst points to output matrix - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_scale_q15( - const arm_matrix_instance_q15 * pSrc, - q15_t scaleFract, - int32_t shift, - arm_matrix_instance_q15 * pDst); - - /** - * @brief Q31 matrix scaling. - * @param[in] *pSrc points to input matrix - * @param[in] scaleFract fractional portion of the scale factor - * @param[in] shift number of bits to shift the result by - * @param[out] *pDst points to output matrix structure - * @return The function returns either - * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. - */ - - arm_status arm_mat_scale_q31( - const arm_matrix_instance_q31 * pSrc, - q31_t scaleFract, - int32_t shift, - arm_matrix_instance_q31 * pDst); - - - /** - * @brief Q31 matrix initialization. - * @param[in,out] *S points to an instance of the floating-point matrix structure. - * @param[in] nRows number of rows in the matrix. - * @param[in] nColumns number of columns in the matrix. - * @param[in] *pData points to the matrix data array. - * @return none - */ - - void arm_mat_init_q31( - arm_matrix_instance_q31 * S, - uint16_t nRows, - uint16_t nColumns, - q31_t * pData); - - /** - * @brief Q15 matrix initialization. - * @param[in,out] *S points to an instance of the floating-point matrix structure. - * @param[in] nRows number of rows in the matrix. - * @param[in] nColumns number of columns in the matrix. - * @param[in] *pData points to the matrix data array. - * @return none - */ - - void arm_mat_init_q15( - arm_matrix_instance_q15 * S, - uint16_t nRows, - uint16_t nColumns, - q15_t * pData); - - /** - * @brief Floating-point matrix initialization. - * @param[in,out] *S points to an instance of the floating-point matrix structure. - * @param[in] nRows number of rows in the matrix. - * @param[in] nColumns number of columns in the matrix. - * @param[in] *pData points to the matrix data array. - * @return none - */ - - void arm_mat_init_f32( - arm_matrix_instance_f32 * S, - uint16_t nRows, - uint16_t nColumns, - float32_t * pData); - - - - /** - * @brief Instance structure for the Q15 PID Control. - */ - typedef struct - { - q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ -#ifdef ARM_MATH_CM0_FAMILY - q15_t A1; - q15_t A2; -#else - q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/ -#endif - q15_t state[3]; /**< The state array of length 3. */ - q15_t Kp; /**< The proportional gain. */ - q15_t Ki; /**< The integral gain. */ - q15_t Kd; /**< The derivative gain. */ - } arm_pid_instance_q15; - - /** - * @brief Instance structure for the Q31 PID Control. - */ - typedef struct - { - q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ - q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ - q31_t A2; /**< The derived gain, A2 = Kd . */ - q31_t state[3]; /**< The state array of length 3. */ - q31_t Kp; /**< The proportional gain. */ - q31_t Ki; /**< The integral gain. */ - q31_t Kd; /**< The derivative gain. */ - - } arm_pid_instance_q31; - - /** - * @brief Instance structure for the floating-point PID Control. - */ - typedef struct - { - float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ - float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ - float32_t A2; /**< The derived gain, A2 = Kd . */ - float32_t state[3]; /**< The state array of length 3. */ - float32_t Kp; /**< The proportional gain. */ - float32_t Ki; /**< The integral gain. */ - float32_t Kd; /**< The derivative gain. */ - } arm_pid_instance_f32; - - - - /** - * @brief Initialization function for the floating-point PID Control. - * @param[in,out] *S points to an instance of the PID structure. - * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. - * @return none. - */ - void arm_pid_init_f32( - arm_pid_instance_f32 * S, - int32_t resetStateFlag); - - /** - * @brief Reset function for the floating-point PID Control. - * @param[in,out] *S is an instance of the floating-point PID Control structure - * @return none - */ - void arm_pid_reset_f32( - arm_pid_instance_f32 * S); - - - /** - * @brief Initialization function for the Q31 PID Control. - * @param[in,out] *S points to an instance of the Q15 PID structure. - * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. - * @return none. - */ - void arm_pid_init_q31( - arm_pid_instance_q31 * S, - int32_t resetStateFlag); - - - /** - * @brief Reset function for the Q31 PID Control. - * @param[in,out] *S points to an instance of the Q31 PID Control structure - * @return none - */ - - void arm_pid_reset_q31( - arm_pid_instance_q31 * S); - - /** - * @brief Initialization function for the Q15 PID Control. - * @param[in,out] *S points to an instance of the Q15 PID structure. - * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. - * @return none. - */ - void arm_pid_init_q15( - arm_pid_instance_q15 * S, - int32_t resetStateFlag); - - /** - * @brief Reset function for the Q15 PID Control. - * @param[in,out] *S points to an instance of the q15 PID Control structure - * @return none - */ - void arm_pid_reset_q15( - arm_pid_instance_q15 * S); - - - /** - * @brief Instance structure for the floating-point Linear Interpolate function. - */ - typedef struct - { - uint32_t nValues; /**< nValues */ - float32_t x1; /**< x1 */ - float32_t xSpacing; /**< xSpacing */ - float32_t *pYData; /**< pointer to the table of Y values */ - } arm_linear_interp_instance_f32; - - /** - * @brief Instance structure for the floating-point bilinear interpolation function. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows in the data table. */ - uint16_t numCols; /**< number of columns in the data table. */ - float32_t *pData; /**< points to the data table. */ - } arm_bilinear_interp_instance_f32; - - /** - * @brief Instance structure for the Q31 bilinear interpolation function. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows in the data table. */ - uint16_t numCols; /**< number of columns in the data table. */ - q31_t *pData; /**< points to the data table. */ - } arm_bilinear_interp_instance_q31; - - /** - * @brief Instance structure for the Q15 bilinear interpolation function. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows in the data table. */ - uint16_t numCols; /**< number of columns in the data table. */ - q15_t *pData; /**< points to the data table. */ - } arm_bilinear_interp_instance_q15; - - /** - * @brief Instance structure for the Q15 bilinear interpolation function. - */ - - typedef struct - { - uint16_t numRows; /**< number of rows in the data table. */ - uint16_t numCols; /**< number of columns in the data table. */ - q7_t *pData; /**< points to the data table. */ - } arm_bilinear_interp_instance_q7; - - - /** - * @brief Q7 vector multiplication. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_mult_q7( - q7_t * pSrcA, - q7_t * pSrcB, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Q15 vector multiplication. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_mult_q15( - q15_t * pSrcA, - q15_t * pSrcB, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Q31 vector multiplication. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_mult_q31( - q31_t * pSrcA, - q31_t * pSrcB, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Floating-point vector multiplication. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_mult_f32( - float32_t * pSrcA, - float32_t * pSrcB, - float32_t * pDst, - uint32_t blockSize); - - - - - - - /** - * @brief Instance structure for the Q15 CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ - uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ - q15_t *pTwiddle; /**< points to the Sin twiddle factor table. */ - uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ - } arm_cfft_radix2_instance_q15; - - arm_status arm_cfft_radix2_init_q15( - arm_cfft_radix2_instance_q15 * S, - uint16_t fftLen, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - - void arm_cfft_radix2_q15( - const arm_cfft_radix2_instance_q15 * S, - q15_t * pSrc); - - - - /** - * @brief Instance structure for the Q15 CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ - uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ - q15_t *pTwiddle; /**< points to the twiddle factor table. */ - uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ - } arm_cfft_radix4_instance_q15; - - arm_status arm_cfft_radix4_init_q15( - arm_cfft_radix4_instance_q15 * S, - uint16_t fftLen, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - - void arm_cfft_radix4_q15( - const arm_cfft_radix4_instance_q15 * S, - q15_t * pSrc); - - /** - * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ - uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ - q31_t *pTwiddle; /**< points to the Twiddle factor table. */ - uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ - } arm_cfft_radix2_instance_q31; - - arm_status arm_cfft_radix2_init_q31( - arm_cfft_radix2_instance_q31 * S, - uint16_t fftLen, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - - void arm_cfft_radix2_q31( - const arm_cfft_radix2_instance_q31 * S, - q31_t * pSrc); - - /** - * @brief Instance structure for the Q31 CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ - uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ - q31_t *pTwiddle; /**< points to the twiddle factor table. */ - uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ - } arm_cfft_radix4_instance_q31; - - - void arm_cfft_radix4_q31( - const arm_cfft_radix4_instance_q31 * S, - q31_t * pSrc); - - arm_status arm_cfft_radix4_init_q31( - arm_cfft_radix4_instance_q31 * S, - uint16_t fftLen, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - - /** - * @brief Instance structure for the floating-point CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ - uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ - float32_t *pTwiddle; /**< points to the Twiddle factor table. */ - uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ - float32_t onebyfftLen; /**< value of 1/fftLen. */ - } arm_cfft_radix2_instance_f32; - -/* Deprecated */ - arm_status arm_cfft_radix2_init_f32( - arm_cfft_radix2_instance_f32 * S, - uint16_t fftLen, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - -/* Deprecated */ - void arm_cfft_radix2_f32( - const arm_cfft_radix2_instance_f32 * S, - float32_t * pSrc); - - /** - * @brief Instance structure for the floating-point CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */ - uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */ - float32_t *pTwiddle; /**< points to the Twiddle factor table. */ - uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */ - float32_t onebyfftLen; /**< value of 1/fftLen. */ - } arm_cfft_radix4_instance_f32; - -/* Deprecated */ - arm_status arm_cfft_radix4_init_f32( - arm_cfft_radix4_instance_f32 * S, - uint16_t fftLen, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - -/* Deprecated */ - void arm_cfft_radix4_f32( - const arm_cfft_radix4_instance_f32 * S, - float32_t * pSrc); - - /** - * @brief Instance structure for the floating-point CFFT/CIFFT function. - */ - - typedef struct - { - uint16_t fftLen; /**< length of the FFT. */ - const float32_t *pTwiddle; /**< points to the Twiddle factor table. */ - const uint16_t *pBitRevTable; /**< points to the bit reversal table. */ - uint16_t bitRevLength; /**< bit reversal table length. */ - } arm_cfft_instance_f32; - - void arm_cfft_f32( - const arm_cfft_instance_f32 * S, - float32_t * p1, - uint8_t ifftFlag, - uint8_t bitReverseFlag); - - /** - * @brief Instance structure for the Q15 RFFT/RIFFT function. - */ - - typedef struct - { - uint32_t fftLenReal; /**< length of the real FFT. */ - uint32_t fftLenBy2; /**< length of the complex FFT. */ - uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ - uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ - uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - q15_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ - q15_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ - arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */ - } arm_rfft_instance_q15; - - arm_status arm_rfft_init_q15( - arm_rfft_instance_q15 * S, - arm_cfft_radix4_instance_q15 * S_CFFT, - uint32_t fftLenReal, - uint32_t ifftFlagR, - uint32_t bitReverseFlag); - - void arm_rfft_q15( - const arm_rfft_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst); - - /** - * @brief Instance structure for the Q31 RFFT/RIFFT function. - */ - - typedef struct - { - uint32_t fftLenReal; /**< length of the real FFT. */ - uint32_t fftLenBy2; /**< length of the complex FFT. */ - uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ - uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ - uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - q31_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ - q31_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ - arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */ - } arm_rfft_instance_q31; - - arm_status arm_rfft_init_q31( - arm_rfft_instance_q31 * S, - arm_cfft_radix4_instance_q31 * S_CFFT, - uint32_t fftLenReal, - uint32_t ifftFlagR, - uint32_t bitReverseFlag); - - void arm_rfft_q31( - const arm_rfft_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst); - - /** - * @brief Instance structure for the floating-point RFFT/RIFFT function. - */ - - typedef struct - { - uint32_t fftLenReal; /**< length of the real FFT. */ - uint16_t fftLenBy2; /**< length of the complex FFT. */ - uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */ - uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */ - uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */ - float32_t *pTwiddleAReal; /**< points to the real twiddle factor table. */ - float32_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */ - arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */ - } arm_rfft_instance_f32; - - arm_status arm_rfft_init_f32( - arm_rfft_instance_f32 * S, - arm_cfft_radix4_instance_f32 * S_CFFT, - uint32_t fftLenReal, - uint32_t ifftFlagR, - uint32_t bitReverseFlag); - - void arm_rfft_f32( - const arm_rfft_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst); - - /** - * @brief Instance structure for the floating-point RFFT/RIFFT function. - */ - -typedef struct - { - arm_cfft_instance_f32 Sint; /**< Internal CFFT structure. */ - uint16_t fftLenRFFT; /**< length of the real sequence */ - float32_t * pTwiddleRFFT; /**< Twiddle factors real stage */ - } arm_rfft_fast_instance_f32 ; - -arm_status arm_rfft_fast_init_f32 ( - arm_rfft_fast_instance_f32 * S, - uint16_t fftLen); - -void arm_rfft_fast_f32( - arm_rfft_fast_instance_f32 * S, - float32_t * p, float32_t * pOut, - uint8_t ifftFlag); - - /** - * @brief Instance structure for the floating-point DCT4/IDCT4 function. - */ - - typedef struct - { - uint16_t N; /**< length of the DCT4. */ - uint16_t Nby2; /**< half of the length of the DCT4. */ - float32_t normalize; /**< normalizing factor. */ - float32_t *pTwiddle; /**< points to the twiddle factor table. */ - float32_t *pCosFactor; /**< points to the cosFactor table. */ - arm_rfft_instance_f32 *pRfft; /**< points to the real FFT instance. */ - arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */ - } arm_dct4_instance_f32; - - /** - * @brief Initialization function for the floating-point DCT4/IDCT4. - * @param[in,out] *S points to an instance of floating-point DCT4/IDCT4 structure. - * @param[in] *S_RFFT points to an instance of floating-point RFFT/RIFFT structure. - * @param[in] *S_CFFT points to an instance of floating-point CFFT/CIFFT structure. - * @param[in] N length of the DCT4. - * @param[in] Nby2 half of the length of the DCT4. - * @param[in] normalize normalizing factor. - * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if fftLenReal is not a supported transform length. - */ - - arm_status arm_dct4_init_f32( - arm_dct4_instance_f32 * S, - arm_rfft_instance_f32 * S_RFFT, - arm_cfft_radix4_instance_f32 * S_CFFT, - uint16_t N, - uint16_t Nby2, - float32_t normalize); - - /** - * @brief Processing function for the floating-point DCT4/IDCT4. - * @param[in] *S points to an instance of the floating-point DCT4/IDCT4 structure. - * @param[in] *pState points to state buffer. - * @param[in,out] *pInlineBuffer points to the in-place input and output buffer. - * @return none. - */ - - void arm_dct4_f32( - const arm_dct4_instance_f32 * S, - float32_t * pState, - float32_t * pInlineBuffer); - - /** - * @brief Instance structure for the Q31 DCT4/IDCT4 function. - */ - - typedef struct - { - uint16_t N; /**< length of the DCT4. */ - uint16_t Nby2; /**< half of the length of the DCT4. */ - q31_t normalize; /**< normalizing factor. */ - q31_t *pTwiddle; /**< points to the twiddle factor table. */ - q31_t *pCosFactor; /**< points to the cosFactor table. */ - arm_rfft_instance_q31 *pRfft; /**< points to the real FFT instance. */ - arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */ - } arm_dct4_instance_q31; - - /** - * @brief Initialization function for the Q31 DCT4/IDCT4. - * @param[in,out] *S points to an instance of Q31 DCT4/IDCT4 structure. - * @param[in] *S_RFFT points to an instance of Q31 RFFT/RIFFT structure - * @param[in] *S_CFFT points to an instance of Q31 CFFT/CIFFT structure - * @param[in] N length of the DCT4. - * @param[in] Nby2 half of the length of the DCT4. - * @param[in] normalize normalizing factor. - * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length. - */ - - arm_status arm_dct4_init_q31( - arm_dct4_instance_q31 * S, - arm_rfft_instance_q31 * S_RFFT, - arm_cfft_radix4_instance_q31 * S_CFFT, - uint16_t N, - uint16_t Nby2, - q31_t normalize); - - /** - * @brief Processing function for the Q31 DCT4/IDCT4. - * @param[in] *S points to an instance of the Q31 DCT4 structure. - * @param[in] *pState points to state buffer. - * @param[in,out] *pInlineBuffer points to the in-place input and output buffer. - * @return none. - */ - - void arm_dct4_q31( - const arm_dct4_instance_q31 * S, - q31_t * pState, - q31_t * pInlineBuffer); - - /** - * @brief Instance structure for the Q15 DCT4/IDCT4 function. - */ - - typedef struct - { - uint16_t N; /**< length of the DCT4. */ - uint16_t Nby2; /**< half of the length of the DCT4. */ - q15_t normalize; /**< normalizing factor. */ - q15_t *pTwiddle; /**< points to the twiddle factor table. */ - q15_t *pCosFactor; /**< points to the cosFactor table. */ - arm_rfft_instance_q15 *pRfft; /**< points to the real FFT instance. */ - arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */ - } arm_dct4_instance_q15; - - /** - * @brief Initialization function for the Q15 DCT4/IDCT4. - * @param[in,out] *S points to an instance of Q15 DCT4/IDCT4 structure. - * @param[in] *S_RFFT points to an instance of Q15 RFFT/RIFFT structure. - * @param[in] *S_CFFT points to an instance of Q15 CFFT/CIFFT structure. - * @param[in] N length of the DCT4. - * @param[in] Nby2 half of the length of the DCT4. - * @param[in] normalize normalizing factor. - * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if N is not a supported transform length. - */ - - arm_status arm_dct4_init_q15( - arm_dct4_instance_q15 * S, - arm_rfft_instance_q15 * S_RFFT, - arm_cfft_radix4_instance_q15 * S_CFFT, - uint16_t N, - uint16_t Nby2, - q15_t normalize); - - /** - * @brief Processing function for the Q15 DCT4/IDCT4. - * @param[in] *S points to an instance of the Q15 DCT4 structure. - * @param[in] *pState points to state buffer. - * @param[in,out] *pInlineBuffer points to the in-place input and output buffer. - * @return none. - */ - - void arm_dct4_q15( - const arm_dct4_instance_q15 * S, - q15_t * pState, - q15_t * pInlineBuffer); - - /** - * @brief Floating-point vector addition. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_add_f32( - float32_t * pSrcA, - float32_t * pSrcB, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Q7 vector addition. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_add_q7( - q7_t * pSrcA, - q7_t * pSrcB, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Q15 vector addition. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_add_q15( - q15_t * pSrcA, - q15_t * pSrcB, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Q31 vector addition. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_add_q31( - q31_t * pSrcA, - q31_t * pSrcB, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Floating-point vector subtraction. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_sub_f32( - float32_t * pSrcA, - float32_t * pSrcB, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Q7 vector subtraction. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_sub_q7( - q7_t * pSrcA, - q7_t * pSrcB, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Q15 vector subtraction. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_sub_q15( - q15_t * pSrcA, - q15_t * pSrcB, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Q31 vector subtraction. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_sub_q31( - q31_t * pSrcA, - q31_t * pSrcB, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Multiplies a floating-point vector by a scalar. - * @param[in] *pSrc points to the input vector - * @param[in] scale scale factor to be applied - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_scale_f32( - float32_t * pSrc, - float32_t scale, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Multiplies a Q7 vector by a scalar. - * @param[in] *pSrc points to the input vector - * @param[in] scaleFract fractional portion of the scale value - * @param[in] shift number of bits to shift the result by - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_scale_q7( - q7_t * pSrc, - q7_t scaleFract, - int8_t shift, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Multiplies a Q15 vector by a scalar. - * @param[in] *pSrc points to the input vector - * @param[in] scaleFract fractional portion of the scale value - * @param[in] shift number of bits to shift the result by - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_scale_q15( - q15_t * pSrc, - q15_t scaleFract, - int8_t shift, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Multiplies a Q31 vector by a scalar. - * @param[in] *pSrc points to the input vector - * @param[in] scaleFract fractional portion of the scale value - * @param[in] shift number of bits to shift the result by - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_scale_q31( - q31_t * pSrc, - q31_t scaleFract, - int8_t shift, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Q7 vector absolute value. - * @param[in] *pSrc points to the input buffer - * @param[out] *pDst points to the output buffer - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_abs_q7( - q7_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Floating-point vector absolute value. - * @param[in] *pSrc points to the input buffer - * @param[out] *pDst points to the output buffer - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_abs_f32( - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Q15 vector absolute value. - * @param[in] *pSrc points to the input buffer - * @param[out] *pDst points to the output buffer - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_abs_q15( - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Q31 vector absolute value. - * @param[in] *pSrc points to the input buffer - * @param[out] *pDst points to the output buffer - * @param[in] blockSize number of samples in each vector - * @return none. - */ - - void arm_abs_q31( - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Dot product of floating-point vectors. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] blockSize number of samples in each vector - * @param[out] *result output result returned here - * @return none. - */ - - void arm_dot_prod_f32( - float32_t * pSrcA, - float32_t * pSrcB, - uint32_t blockSize, - float32_t * result); - - /** - * @brief Dot product of Q7 vectors. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] blockSize number of samples in each vector - * @param[out] *result output result returned here - * @return none. - */ - - void arm_dot_prod_q7( - q7_t * pSrcA, - q7_t * pSrcB, - uint32_t blockSize, - q31_t * result); - - /** - * @brief Dot product of Q15 vectors. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] blockSize number of samples in each vector - * @param[out] *result output result returned here - * @return none. - */ - - void arm_dot_prod_q15( - q15_t * pSrcA, - q15_t * pSrcB, - uint32_t blockSize, - q63_t * result); - - /** - * @brief Dot product of Q31 vectors. - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] blockSize number of samples in each vector - * @param[out] *result output result returned here - * @return none. - */ - - void arm_dot_prod_q31( - q31_t * pSrcA, - q31_t * pSrcB, - uint32_t blockSize, - q63_t * result); - - /** - * @brief Shifts the elements of a Q7 vector a specified number of bits. - * @param[in] *pSrc points to the input vector - * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_shift_q7( - q7_t * pSrc, - int8_t shiftBits, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Shifts the elements of a Q15 vector a specified number of bits. - * @param[in] *pSrc points to the input vector - * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_shift_q15( - q15_t * pSrc, - int8_t shiftBits, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Shifts the elements of a Q31 vector a specified number of bits. - * @param[in] *pSrc points to the input vector - * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_shift_q31( - q31_t * pSrc, - int8_t shiftBits, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Adds a constant offset to a floating-point vector. - * @param[in] *pSrc points to the input vector - * @param[in] offset is the offset to be added - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_offset_f32( - float32_t * pSrc, - float32_t offset, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Adds a constant offset to a Q7 vector. - * @param[in] *pSrc points to the input vector - * @param[in] offset is the offset to be added - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_offset_q7( - q7_t * pSrc, - q7_t offset, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Adds a constant offset to a Q15 vector. - * @param[in] *pSrc points to the input vector - * @param[in] offset is the offset to be added - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_offset_q15( - q15_t * pSrc, - q15_t offset, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Adds a constant offset to a Q31 vector. - * @param[in] *pSrc points to the input vector - * @param[in] offset is the offset to be added - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_offset_q31( - q31_t * pSrc, - q31_t offset, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Negates the elements of a floating-point vector. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_negate_f32( - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Negates the elements of a Q7 vector. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_negate_q7( - q7_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Negates the elements of a Q15 vector. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_negate_q15( - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Negates the elements of a Q31 vector. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] blockSize number of samples in the vector - * @return none. - */ - - void arm_negate_q31( - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - /** - * @brief Copies the elements of a floating-point vector. - * @param[in] *pSrc input pointer - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_copy_f32( - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Copies the elements of a Q7 vector. - * @param[in] *pSrc input pointer - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_copy_q7( - q7_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Copies the elements of a Q15 vector. - * @param[in] *pSrc input pointer - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_copy_q15( - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Copies the elements of a Q31 vector. - * @param[in] *pSrc input pointer - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_copy_q31( - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - /** - * @brief Fills a constant value into a floating-point vector. - * @param[in] value input value to be filled - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_fill_f32( - float32_t value, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Fills a constant value into a Q7 vector. - * @param[in] value input value to be filled - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_fill_q7( - q7_t value, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Fills a constant value into a Q15 vector. - * @param[in] value input value to be filled - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_fill_q15( - q15_t value, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Fills a constant value into a Q31 vector. - * @param[in] value input value to be filled - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_fill_q31( - q31_t value, - q31_t * pDst, - uint32_t blockSize); - -/** - * @brief Convolution of floating-point sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1. - * @return none. - */ - - void arm_conv_f32( - float32_t * pSrcA, - uint32_t srcALen, - float32_t * pSrcB, - uint32_t srcBLen, - float32_t * pDst); - - - /** - * @brief Convolution of Q15 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen). - * @return none. - */ - - - void arm_conv_opt_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - q15_t * pScratch1, - q15_t * pScratch2); - - -/** - * @brief Convolution of Q15 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1. - * @return none. - */ - - void arm_conv_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst); - - /** - * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @return none. - */ - - void arm_conv_fast_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst); - - /** - * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen). - * @return none. - */ - - void arm_conv_fast_opt_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - q15_t * pScratch1, - q15_t * pScratch2); - - - - /** - * @brief Convolution of Q31 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @return none. - */ - - void arm_conv_q31( - q31_t * pSrcA, - uint32_t srcALen, - q31_t * pSrcB, - uint32_t srcBLen, - q31_t * pDst); - - /** - * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @return none. - */ - - void arm_conv_fast_q31( - q31_t * pSrcA, - uint32_t srcALen, - q31_t * pSrcB, - uint32_t srcBLen, - q31_t * pDst); - - - /** - * @brief Convolution of Q7 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). - * @return none. - */ - - void arm_conv_opt_q7( - q7_t * pSrcA, - uint32_t srcALen, - q7_t * pSrcB, - uint32_t srcBLen, - q7_t * pDst, - q15_t * pScratch1, - q15_t * pScratch2); - - - - /** - * @brief Convolution of Q7 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1. - * @return none. - */ - - void arm_conv_q7( - q7_t * pSrcA, - uint32_t srcALen, - q7_t * pSrcB, - uint32_t srcBLen, - q7_t * pDst); - - - /** - * @brief Partial convolution of floating-point sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_f32( - float32_t * pSrcA, - uint32_t srcALen, - float32_t * pSrcB, - uint32_t srcBLen, - float32_t * pDst, - uint32_t firstIndex, - uint32_t numPoints); - - /** - * @brief Partial convolution of Q15 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen). - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_opt_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - uint32_t firstIndex, - uint32_t numPoints, - q15_t * pScratch1, - q15_t * pScratch2); - - -/** - * @brief Partial convolution of Q15 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - uint32_t firstIndex, - uint32_t numPoints); - - /** - * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_fast_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - uint32_t firstIndex, - uint32_t numPoints); - - - /** - * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen). - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_fast_opt_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - uint32_t firstIndex, - uint32_t numPoints, - q15_t * pScratch1, - q15_t * pScratch2); - - - /** - * @brief Partial convolution of Q31 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_q31( - q31_t * pSrcA, - uint32_t srcALen, - q31_t * pSrcB, - uint32_t srcBLen, - q31_t * pDst, - uint32_t firstIndex, - uint32_t numPoints); - - - /** - * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_fast_q31( - q31_t * pSrcA, - uint32_t srcALen, - q31_t * pSrcB, - uint32_t srcBLen, - q31_t * pDst, - uint32_t firstIndex, - uint32_t numPoints); - - - /** - * @brief Partial convolution of Q7 sequences - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_opt_q7( - q7_t * pSrcA, - uint32_t srcALen, - q7_t * pSrcB, - uint32_t srcBLen, - q7_t * pDst, - uint32_t firstIndex, - uint32_t numPoints, - q15_t * pScratch1, - q15_t * pScratch2); - - -/** - * @brief Partial convolution of Q7 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data - * @param[in] firstIndex is the first output sample to start with. - * @param[in] numPoints is the number of output points to be computed. - * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2]. - */ - - arm_status arm_conv_partial_q7( - q7_t * pSrcA, - uint32_t srcALen, - q7_t * pSrcB, - uint32_t srcBLen, - q7_t * pDst, - uint32_t firstIndex, - uint32_t numPoints); - - - - /** - * @brief Instance structure for the Q15 FIR decimator. - */ - - typedef struct - { - uint8_t M; /**< decimation factor. */ - uint16_t numTaps; /**< number of coefficients in the filter. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - } arm_fir_decimate_instance_q15; - - /** - * @brief Instance structure for the Q31 FIR decimator. - */ - - typedef struct - { - uint8_t M; /**< decimation factor. */ - uint16_t numTaps; /**< number of coefficients in the filter. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - - } arm_fir_decimate_instance_q31; - - /** - * @brief Instance structure for the floating-point FIR decimator. - */ - - typedef struct - { - uint8_t M; /**< decimation factor. */ - uint16_t numTaps; /**< number of coefficients in the filter. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - - } arm_fir_decimate_instance_f32; - - - - /** - * @brief Processing function for the floating-point FIR decimator. - * @param[in] *S points to an instance of the floating-point FIR decimator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of input samples to process per call. - * @return none - */ - - void arm_fir_decimate_f32( - const arm_fir_decimate_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - - /** - * @brief Initialization function for the floating-point FIR decimator. - * @param[in,out] *S points to an instance of the floating-point FIR decimator structure. - * @param[in] numTaps number of coefficients in the filter. - * @param[in] M decimation factor. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of input samples to process per call. - * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if - * blockSize is not a multiple of M. - */ - - arm_status arm_fir_decimate_init_f32( - arm_fir_decimate_instance_f32 * S, - uint16_t numTaps, - uint8_t M, - float32_t * pCoeffs, - float32_t * pState, - uint32_t blockSize); - - /** - * @brief Processing function for the Q15 FIR decimator. - * @param[in] *S points to an instance of the Q15 FIR decimator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of input samples to process per call. - * @return none - */ - - void arm_fir_decimate_q15( - const arm_fir_decimate_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4. - * @param[in] *S points to an instance of the Q15 FIR decimator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of input samples to process per call. - * @return none - */ - - void arm_fir_decimate_fast_q15( - const arm_fir_decimate_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - - - /** - * @brief Initialization function for the Q15 FIR decimator. - * @param[in,out] *S points to an instance of the Q15 FIR decimator structure. - * @param[in] numTaps number of coefficients in the filter. - * @param[in] M decimation factor. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of input samples to process per call. - * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if - * blockSize is not a multiple of M. - */ - - arm_status arm_fir_decimate_init_q15( - arm_fir_decimate_instance_q15 * S, - uint16_t numTaps, - uint8_t M, - q15_t * pCoeffs, - q15_t * pState, - uint32_t blockSize); - - /** - * @brief Processing function for the Q31 FIR decimator. - * @param[in] *S points to an instance of the Q31 FIR decimator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of input samples to process per call. - * @return none - */ - - void arm_fir_decimate_q31( - const arm_fir_decimate_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4. - * @param[in] *S points to an instance of the Q31 FIR decimator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of input samples to process per call. - * @return none - */ - - void arm_fir_decimate_fast_q31( - arm_fir_decimate_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - - /** - * @brief Initialization function for the Q31 FIR decimator. - * @param[in,out] *S points to an instance of the Q31 FIR decimator structure. - * @param[in] numTaps number of coefficients in the filter. - * @param[in] M decimation factor. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of input samples to process per call. - * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if - * blockSize is not a multiple of M. - */ - - arm_status arm_fir_decimate_init_q31( - arm_fir_decimate_instance_q31 * S, - uint16_t numTaps, - uint8_t M, - q31_t * pCoeffs, - q31_t * pState, - uint32_t blockSize); - - - - /** - * @brief Instance structure for the Q15 FIR interpolator. - */ - - typedef struct - { - uint8_t L; /**< upsample factor. */ - uint16_t phaseLength; /**< length of each polyphase filter component. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ - q15_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */ - } arm_fir_interpolate_instance_q15; - - /** - * @brief Instance structure for the Q31 FIR interpolator. - */ - - typedef struct - { - uint8_t L; /**< upsample factor. */ - uint16_t phaseLength; /**< length of each polyphase filter component. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ - q31_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */ - } arm_fir_interpolate_instance_q31; - - /** - * @brief Instance structure for the floating-point FIR interpolator. - */ - - typedef struct - { - uint8_t L; /**< upsample factor. */ - uint16_t phaseLength; /**< length of each polyphase filter component. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */ - float32_t *pState; /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */ - } arm_fir_interpolate_instance_f32; - - - /** - * @brief Processing function for the Q15 FIR interpolator. - * @param[in] *S points to an instance of the Q15 FIR interpolator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_interpolate_q15( - const arm_fir_interpolate_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - - /** - * @brief Initialization function for the Q15 FIR interpolator. - * @param[in,out] *S points to an instance of the Q15 FIR interpolator structure. - * @param[in] L upsample factor. - * @param[in] numTaps number of filter coefficients in the filter. - * @param[in] *pCoeffs points to the filter coefficient buffer. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of input samples to process per call. - * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if - * the filter length numTaps is not a multiple of the interpolation factor L. - */ - - arm_status arm_fir_interpolate_init_q15( - arm_fir_interpolate_instance_q15 * S, - uint8_t L, - uint16_t numTaps, - q15_t * pCoeffs, - q15_t * pState, - uint32_t blockSize); - - /** - * @brief Processing function for the Q31 FIR interpolator. - * @param[in] *S points to an instance of the Q15 FIR interpolator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_interpolate_q31( - const arm_fir_interpolate_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q31 FIR interpolator. - * @param[in,out] *S points to an instance of the Q31 FIR interpolator structure. - * @param[in] L upsample factor. - * @param[in] numTaps number of filter coefficients in the filter. - * @param[in] *pCoeffs points to the filter coefficient buffer. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of input samples to process per call. - * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if - * the filter length numTaps is not a multiple of the interpolation factor L. - */ - - arm_status arm_fir_interpolate_init_q31( - arm_fir_interpolate_instance_q31 * S, - uint8_t L, - uint16_t numTaps, - q31_t * pCoeffs, - q31_t * pState, - uint32_t blockSize); - - - /** - * @brief Processing function for the floating-point FIR interpolator. - * @param[in] *S points to an instance of the floating-point FIR interpolator structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_interpolate_f32( - const arm_fir_interpolate_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the floating-point FIR interpolator. - * @param[in,out] *S points to an instance of the floating-point FIR interpolator structure. - * @param[in] L upsample factor. - * @param[in] numTaps number of filter coefficients in the filter. - * @param[in] *pCoeffs points to the filter coefficient buffer. - * @param[in] *pState points to the state buffer. - * @param[in] blockSize number of input samples to process per call. - * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if - * the filter length numTaps is not a multiple of the interpolation factor L. - */ - - arm_status arm_fir_interpolate_init_f32( - arm_fir_interpolate_instance_f32 * S, - uint8_t L, - uint16_t numTaps, - float32_t * pCoeffs, - float32_t * pState, - uint32_t blockSize); - - /** - * @brief Instance structure for the high precision Q31 Biquad cascade filter. - */ - - typedef struct - { - uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ - q63_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */ - q31_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ - uint8_t postShift; /**< additional shift, in bits, applied to each output sample. */ - - } arm_biquad_cas_df1_32x64_ins_q31; - - - /** - * @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cas_df1_32x64_q31( - const arm_biquad_cas_df1_32x64_ins_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - - /** - * @param[in,out] *S points to an instance of the high precision Q31 Biquad cascade filter structure. - * @param[in] numStages number of 2nd order stages in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] postShift shift to be applied to the output. Varies according to the coefficients format - * @return none - */ - - void arm_biquad_cas_df1_32x64_init_q31( - arm_biquad_cas_df1_32x64_ins_q31 * S, - uint8_t numStages, - q31_t * pCoeffs, - q63_t * pState, - uint8_t postShift); - - - - /** - * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. - */ - - typedef struct - { - uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ - float32_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */ - float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ - } arm_biquad_cascade_df2T_instance_f32; - - - /** - * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. - * @param[in] *S points to an instance of the filter data structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_biquad_cascade_df2T_f32( - const arm_biquad_cascade_df2T_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - - /** - * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter. - * @param[in,out] *S points to an instance of the filter data structure. - * @param[in] numStages number of 2nd order stages in the filter. - * @param[in] *pCoeffs points to the filter coefficients. - * @param[in] *pState points to the state buffer. - * @return none - */ - - void arm_biquad_cascade_df2T_init_f32( - arm_biquad_cascade_df2T_instance_f32 * S, - uint8_t numStages, - float32_t * pCoeffs, - float32_t * pState); - - - - /** - * @brief Instance structure for the Q15 FIR lattice filter. - */ - - typedef struct - { - uint16_t numStages; /**< number of filter stages. */ - q15_t *pState; /**< points to the state variable array. The array is of length numStages. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ - } arm_fir_lattice_instance_q15; - - /** - * @brief Instance structure for the Q31 FIR lattice filter. - */ - - typedef struct - { - uint16_t numStages; /**< number of filter stages. */ - q31_t *pState; /**< points to the state variable array. The array is of length numStages. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ - } arm_fir_lattice_instance_q31; - - /** - * @brief Instance structure for the floating-point FIR lattice filter. - */ - - typedef struct - { - uint16_t numStages; /**< number of filter stages. */ - float32_t *pState; /**< points to the state variable array. The array is of length numStages. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */ - } arm_fir_lattice_instance_f32; - - /** - * @brief Initialization function for the Q15 FIR lattice filter. - * @param[in] *S points to an instance of the Q15 FIR lattice structure. - * @param[in] numStages number of filter stages. - * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages. - * @param[in] *pState points to the state buffer. The array is of length numStages. - * @return none. - */ - - void arm_fir_lattice_init_q15( - arm_fir_lattice_instance_q15 * S, - uint16_t numStages, - q15_t * pCoeffs, - q15_t * pState); - - - /** - * @brief Processing function for the Q15 FIR lattice filter. - * @param[in] *S points to an instance of the Q15 FIR lattice structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - void arm_fir_lattice_q15( - const arm_fir_lattice_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q31 FIR lattice filter. - * @param[in] *S points to an instance of the Q31 FIR lattice structure. - * @param[in] numStages number of filter stages. - * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages. - * @param[in] *pState points to the state buffer. The array is of length numStages. - * @return none. - */ - - void arm_fir_lattice_init_q31( - arm_fir_lattice_instance_q31 * S, - uint16_t numStages, - q31_t * pCoeffs, - q31_t * pState); - - - /** - * @brief Processing function for the Q31 FIR lattice filter. - * @param[in] *S points to an instance of the Q31 FIR lattice structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_fir_lattice_q31( - const arm_fir_lattice_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - -/** - * @brief Initialization function for the floating-point FIR lattice filter. - * @param[in] *S points to an instance of the floating-point FIR lattice structure. - * @param[in] numStages number of filter stages. - * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages. - * @param[in] *pState points to the state buffer. The array is of length numStages. - * @return none. - */ - - void arm_fir_lattice_init_f32( - arm_fir_lattice_instance_f32 * S, - uint16_t numStages, - float32_t * pCoeffs, - float32_t * pState); - - /** - * @brief Processing function for the floating-point FIR lattice filter. - * @param[in] *S points to an instance of the floating-point FIR lattice structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_fir_lattice_f32( - const arm_fir_lattice_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Instance structure for the Q15 IIR lattice filter. - */ - typedef struct - { - uint16_t numStages; /**< number of stages in the filter. */ - q15_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ - q15_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ - q15_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ - } arm_iir_lattice_instance_q15; - - /** - * @brief Instance structure for the Q31 IIR lattice filter. - */ - typedef struct - { - uint16_t numStages; /**< number of stages in the filter. */ - q31_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ - q31_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ - q31_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ - } arm_iir_lattice_instance_q31; - - /** - * @brief Instance structure for the floating-point IIR lattice filter. - */ - typedef struct - { - uint16_t numStages; /**< number of stages in the filter. */ - float32_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */ - float32_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */ - float32_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */ - } arm_iir_lattice_instance_f32; - - /** - * @brief Processing function for the floating-point IIR lattice filter. - * @param[in] *S points to an instance of the floating-point IIR lattice structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_iir_lattice_f32( - const arm_iir_lattice_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @brief Initialization function for the floating-point IIR lattice filter. - * @param[in] *S points to an instance of the floating-point IIR lattice structure. - * @param[in] numStages number of stages in the filter. - * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages. - * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1. - * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize-1. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_iir_lattice_init_f32( - arm_iir_lattice_instance_f32 * S, - uint16_t numStages, - float32_t * pkCoeffs, - float32_t * pvCoeffs, - float32_t * pState, - uint32_t blockSize); - - - /** - * @brief Processing function for the Q31 IIR lattice filter. - * @param[in] *S points to an instance of the Q31 IIR lattice structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_iir_lattice_q31( - const arm_iir_lattice_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - - /** - * @brief Initialization function for the Q31 IIR lattice filter. - * @param[in] *S points to an instance of the Q31 IIR lattice structure. - * @param[in] numStages number of stages in the filter. - * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages. - * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1. - * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_iir_lattice_init_q31( - arm_iir_lattice_instance_q31 * S, - uint16_t numStages, - q31_t * pkCoeffs, - q31_t * pvCoeffs, - q31_t * pState, - uint32_t blockSize); - - - /** - * @brief Processing function for the Q15 IIR lattice filter. - * @param[in] *S points to an instance of the Q15 IIR lattice structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_iir_lattice_q15( - const arm_iir_lattice_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - -/** - * @brief Initialization function for the Q15 IIR lattice filter. - * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure. - * @param[in] numStages number of stages in the filter. - * @param[in] *pkCoeffs points to reflection coefficient buffer. The array is of length numStages. - * @param[in] *pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1. - * @param[in] *pState points to state buffer. The array is of length numStages+blockSize. - * @param[in] blockSize number of samples to process per call. - * @return none. - */ - - void arm_iir_lattice_init_q15( - arm_iir_lattice_instance_q15 * S, - uint16_t numStages, - q15_t * pkCoeffs, - q15_t * pvCoeffs, - q15_t * pState, - uint32_t blockSize); - - /** - * @brief Instance structure for the floating-point LMS filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - float32_t mu; /**< step size that controls filter coefficient updates. */ - } arm_lms_instance_f32; - - /** - * @brief Processing function for floating-point LMS filter. - * @param[in] *S points to an instance of the floating-point LMS filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[in] *pRef points to the block of reference data. - * @param[out] *pOut points to the block of output data. - * @param[out] *pErr points to the block of error data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_f32( - const arm_lms_instance_f32 * S, - float32_t * pSrc, - float32_t * pRef, - float32_t * pOut, - float32_t * pErr, - uint32_t blockSize); - - /** - * @brief Initialization function for floating-point LMS filter. - * @param[in] *S points to an instance of the floating-point LMS filter structure. - * @param[in] numTaps number of filter coefficients. - * @param[in] *pCoeffs points to the coefficient buffer. - * @param[in] *pState points to state buffer. - * @param[in] mu step size that controls filter coefficient updates. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_init_f32( - arm_lms_instance_f32 * S, - uint16_t numTaps, - float32_t * pCoeffs, - float32_t * pState, - float32_t mu, - uint32_t blockSize); - - /** - * @brief Instance structure for the Q15 LMS filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - q15_t mu; /**< step size that controls filter coefficient updates. */ - uint32_t postShift; /**< bit shift applied to coefficients. */ - } arm_lms_instance_q15; - - - /** - * @brief Initialization function for the Q15 LMS filter. - * @param[in] *S points to an instance of the Q15 LMS filter structure. - * @param[in] numTaps number of filter coefficients. - * @param[in] *pCoeffs points to the coefficient buffer. - * @param[in] *pState points to the state buffer. - * @param[in] mu step size that controls filter coefficient updates. - * @param[in] blockSize number of samples to process. - * @param[in] postShift bit shift applied to coefficients. - * @return none. - */ - - void arm_lms_init_q15( - arm_lms_instance_q15 * S, - uint16_t numTaps, - q15_t * pCoeffs, - q15_t * pState, - q15_t mu, - uint32_t blockSize, - uint32_t postShift); - - /** - * @brief Processing function for Q15 LMS filter. - * @param[in] *S points to an instance of the Q15 LMS filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[in] *pRef points to the block of reference data. - * @param[out] *pOut points to the block of output data. - * @param[out] *pErr points to the block of error data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_q15( - const arm_lms_instance_q15 * S, - q15_t * pSrc, - q15_t * pRef, - q15_t * pOut, - q15_t * pErr, - uint32_t blockSize); - - - /** - * @brief Instance structure for the Q31 LMS filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - q31_t mu; /**< step size that controls filter coefficient updates. */ - uint32_t postShift; /**< bit shift applied to coefficients. */ - - } arm_lms_instance_q31; - - /** - * @brief Processing function for Q31 LMS filter. - * @param[in] *S points to an instance of the Q15 LMS filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[in] *pRef points to the block of reference data. - * @param[out] *pOut points to the block of output data. - * @param[out] *pErr points to the block of error data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_q31( - const arm_lms_instance_q31 * S, - q31_t * pSrc, - q31_t * pRef, - q31_t * pOut, - q31_t * pErr, - uint32_t blockSize); - - /** - * @brief Initialization function for Q31 LMS filter. - * @param[in] *S points to an instance of the Q31 LMS filter structure. - * @param[in] numTaps number of filter coefficients. - * @param[in] *pCoeffs points to coefficient buffer. - * @param[in] *pState points to state buffer. - * @param[in] mu step size that controls filter coefficient updates. - * @param[in] blockSize number of samples to process. - * @param[in] postShift bit shift applied to coefficients. - * @return none. - */ - - void arm_lms_init_q31( - arm_lms_instance_q31 * S, - uint16_t numTaps, - q31_t * pCoeffs, - q31_t * pState, - q31_t mu, - uint32_t blockSize, - uint32_t postShift); - - /** - * @brief Instance structure for the floating-point normalized LMS filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - float32_t mu; /**< step size that control filter coefficient updates. */ - float32_t energy; /**< saves previous frame energy. */ - float32_t x0; /**< saves previous input sample. */ - } arm_lms_norm_instance_f32; - - /** - * @brief Processing function for floating-point normalized LMS filter. - * @param[in] *S points to an instance of the floating-point normalized LMS filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[in] *pRef points to the block of reference data. - * @param[out] *pOut points to the block of output data. - * @param[out] *pErr points to the block of error data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_norm_f32( - arm_lms_norm_instance_f32 * S, - float32_t * pSrc, - float32_t * pRef, - float32_t * pOut, - float32_t * pErr, - uint32_t blockSize); - - /** - * @brief Initialization function for floating-point normalized LMS filter. - * @param[in] *S points to an instance of the floating-point LMS filter structure. - * @param[in] numTaps number of filter coefficients. - * @param[in] *pCoeffs points to coefficient buffer. - * @param[in] *pState points to state buffer. - * @param[in] mu step size that controls filter coefficient updates. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_norm_init_f32( - arm_lms_norm_instance_f32 * S, - uint16_t numTaps, - float32_t * pCoeffs, - float32_t * pState, - float32_t mu, - uint32_t blockSize); - - - /** - * @brief Instance structure for the Q31 normalized LMS filter. - */ - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - q31_t mu; /**< step size that controls filter coefficient updates. */ - uint8_t postShift; /**< bit shift applied to coefficients. */ - q31_t *recipTable; /**< points to the reciprocal initial value table. */ - q31_t energy; /**< saves previous frame energy. */ - q31_t x0; /**< saves previous input sample. */ - } arm_lms_norm_instance_q31; - - /** - * @brief Processing function for Q31 normalized LMS filter. - * @param[in] *S points to an instance of the Q31 normalized LMS filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[in] *pRef points to the block of reference data. - * @param[out] *pOut points to the block of output data. - * @param[out] *pErr points to the block of error data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_norm_q31( - arm_lms_norm_instance_q31 * S, - q31_t * pSrc, - q31_t * pRef, - q31_t * pOut, - q31_t * pErr, - uint32_t blockSize); - - /** - * @brief Initialization function for Q31 normalized LMS filter. - * @param[in] *S points to an instance of the Q31 normalized LMS filter structure. - * @param[in] numTaps number of filter coefficients. - * @param[in] *pCoeffs points to coefficient buffer. - * @param[in] *pState points to state buffer. - * @param[in] mu step size that controls filter coefficient updates. - * @param[in] blockSize number of samples to process. - * @param[in] postShift bit shift applied to coefficients. - * @return none. - */ - - void arm_lms_norm_init_q31( - arm_lms_norm_instance_q31 * S, - uint16_t numTaps, - q31_t * pCoeffs, - q31_t * pState, - q31_t mu, - uint32_t blockSize, - uint8_t postShift); - - /** - * @brief Instance structure for the Q15 normalized LMS filter. - */ - - typedef struct - { - uint16_t numTaps; /**< Number of coefficients in the filter. */ - q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */ - q15_t mu; /**< step size that controls filter coefficient updates. */ - uint8_t postShift; /**< bit shift applied to coefficients. */ - q15_t *recipTable; /**< Points to the reciprocal initial value table. */ - q15_t energy; /**< saves previous frame energy. */ - q15_t x0; /**< saves previous input sample. */ - } arm_lms_norm_instance_q15; - - /** - * @brief Processing function for Q15 normalized LMS filter. - * @param[in] *S points to an instance of the Q15 normalized LMS filter structure. - * @param[in] *pSrc points to the block of input data. - * @param[in] *pRef points to the block of reference data. - * @param[out] *pOut points to the block of output data. - * @param[out] *pErr points to the block of error data. - * @param[in] blockSize number of samples to process. - * @return none. - */ - - void arm_lms_norm_q15( - arm_lms_norm_instance_q15 * S, - q15_t * pSrc, - q15_t * pRef, - q15_t * pOut, - q15_t * pErr, - uint32_t blockSize); - - - /** - * @brief Initialization function for Q15 normalized LMS filter. - * @param[in] *S points to an instance of the Q15 normalized LMS filter structure. - * @param[in] numTaps number of filter coefficients. - * @param[in] *pCoeffs points to coefficient buffer. - * @param[in] *pState points to state buffer. - * @param[in] mu step size that controls filter coefficient updates. - * @param[in] blockSize number of samples to process. - * @param[in] postShift bit shift applied to coefficients. - * @return none. - */ - - void arm_lms_norm_init_q15( - arm_lms_norm_instance_q15 * S, - uint16_t numTaps, - q15_t * pCoeffs, - q15_t * pState, - q15_t mu, - uint32_t blockSize, - uint8_t postShift); - - /** - * @brief Correlation of floating-point sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @return none. - */ - - void arm_correlate_f32( - float32_t * pSrcA, - uint32_t srcALen, - float32_t * pSrcB, - uint32_t srcBLen, - float32_t * pDst); - - - /** - * @brief Correlation of Q15 sequences - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @return none. - */ - void arm_correlate_opt_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - q15_t * pScratch); - - - /** - * @brief Correlation of Q15 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @return none. - */ - - void arm_correlate_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst); - - /** - * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @return none. - */ - - void arm_correlate_fast_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst); - - - - /** - * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @return none. - */ - - void arm_correlate_fast_opt_q15( - q15_t * pSrcA, - uint32_t srcALen, - q15_t * pSrcB, - uint32_t srcBLen, - q15_t * pDst, - q15_t * pScratch); - - /** - * @brief Correlation of Q31 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @return none. - */ - - void arm_correlate_q31( - q31_t * pSrcA, - uint32_t srcALen, - q31_t * pSrcB, - uint32_t srcBLen, - q31_t * pDst); - - /** - * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4 - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @return none. - */ - - void arm_correlate_fast_q31( - q31_t * pSrcA, - uint32_t srcALen, - q31_t * pSrcB, - uint32_t srcBLen, - q31_t * pDst); - - - - /** - * @brief Correlation of Q7 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2. - * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen). - * @return none. - */ - - void arm_correlate_opt_q7( - q7_t * pSrcA, - uint32_t srcALen, - q7_t * pSrcB, - uint32_t srcBLen, - q7_t * pDst, - q15_t * pScratch1, - q15_t * pScratch2); - - - /** - * @brief Correlation of Q7 sequences. - * @param[in] *pSrcA points to the first input sequence. - * @param[in] srcALen length of the first input sequence. - * @param[in] *pSrcB points to the second input sequence. - * @param[in] srcBLen length of the second input sequence. - * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1. - * @return none. - */ - - void arm_correlate_q7( - q7_t * pSrcA, - uint32_t srcALen, - q7_t * pSrcB, - uint32_t srcBLen, - q7_t * pDst); - - - /** - * @brief Instance structure for the floating-point sparse FIR filter. - */ - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ - float32_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ - float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ - int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ - } arm_fir_sparse_instance_f32; - - /** - * @brief Instance structure for the Q31 sparse FIR filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ - q31_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ - q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ - int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ - } arm_fir_sparse_instance_q31; - - /** - * @brief Instance structure for the Q15 sparse FIR filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ - q15_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ - q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ - int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ - } arm_fir_sparse_instance_q15; - - /** - * @brief Instance structure for the Q7 sparse FIR filter. - */ - - typedef struct - { - uint16_t numTaps; /**< number of coefficients in the filter. */ - uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */ - q7_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */ - q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/ - uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */ - int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */ - } arm_fir_sparse_instance_q7; - - /** - * @brief Processing function for the floating-point sparse FIR filter. - * @param[in] *S points to an instance of the floating-point sparse FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] *pScratchIn points to a temporary buffer of size blockSize. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_sparse_f32( - arm_fir_sparse_instance_f32 * S, - float32_t * pSrc, - float32_t * pDst, - float32_t * pScratchIn, - uint32_t blockSize); - - /** - * @brief Initialization function for the floating-point sparse FIR filter. - * @param[in,out] *S points to an instance of the floating-point sparse FIR structure. - * @param[in] numTaps number of nonzero coefficients in the filter. - * @param[in] *pCoeffs points to the array of filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] *pTapDelay points to the array of offset times. - * @param[in] maxDelay maximum offset time supported. - * @param[in] blockSize number of samples that will be processed per block. - * @return none - */ - - void arm_fir_sparse_init_f32( - arm_fir_sparse_instance_f32 * S, - uint16_t numTaps, - float32_t * pCoeffs, - float32_t * pState, - int32_t * pTapDelay, - uint16_t maxDelay, - uint32_t blockSize); - - /** - * @brief Processing function for the Q31 sparse FIR filter. - * @param[in] *S points to an instance of the Q31 sparse FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] *pScratchIn points to a temporary buffer of size blockSize. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_sparse_q31( - arm_fir_sparse_instance_q31 * S, - q31_t * pSrc, - q31_t * pDst, - q31_t * pScratchIn, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q31 sparse FIR filter. - * @param[in,out] *S points to an instance of the Q31 sparse FIR structure. - * @param[in] numTaps number of nonzero coefficients in the filter. - * @param[in] *pCoeffs points to the array of filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] *pTapDelay points to the array of offset times. - * @param[in] maxDelay maximum offset time supported. - * @param[in] blockSize number of samples that will be processed per block. - * @return none - */ - - void arm_fir_sparse_init_q31( - arm_fir_sparse_instance_q31 * S, - uint16_t numTaps, - q31_t * pCoeffs, - q31_t * pState, - int32_t * pTapDelay, - uint16_t maxDelay, - uint32_t blockSize); - - /** - * @brief Processing function for the Q15 sparse FIR filter. - * @param[in] *S points to an instance of the Q15 sparse FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] *pScratchIn points to a temporary buffer of size blockSize. - * @param[in] *pScratchOut points to a temporary buffer of size blockSize. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_sparse_q15( - arm_fir_sparse_instance_q15 * S, - q15_t * pSrc, - q15_t * pDst, - q15_t * pScratchIn, - q31_t * pScratchOut, - uint32_t blockSize); - - - /** - * @brief Initialization function for the Q15 sparse FIR filter. - * @param[in,out] *S points to an instance of the Q15 sparse FIR structure. - * @param[in] numTaps number of nonzero coefficients in the filter. - * @param[in] *pCoeffs points to the array of filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] *pTapDelay points to the array of offset times. - * @param[in] maxDelay maximum offset time supported. - * @param[in] blockSize number of samples that will be processed per block. - * @return none - */ - - void arm_fir_sparse_init_q15( - arm_fir_sparse_instance_q15 * S, - uint16_t numTaps, - q15_t * pCoeffs, - q15_t * pState, - int32_t * pTapDelay, - uint16_t maxDelay, - uint32_t blockSize); - - /** - * @brief Processing function for the Q7 sparse FIR filter. - * @param[in] *S points to an instance of the Q7 sparse FIR structure. - * @param[in] *pSrc points to the block of input data. - * @param[out] *pDst points to the block of output data - * @param[in] *pScratchIn points to a temporary buffer of size blockSize. - * @param[in] *pScratchOut points to a temporary buffer of size blockSize. - * @param[in] blockSize number of input samples to process per call. - * @return none. - */ - - void arm_fir_sparse_q7( - arm_fir_sparse_instance_q7 * S, - q7_t * pSrc, - q7_t * pDst, - q7_t * pScratchIn, - q31_t * pScratchOut, - uint32_t blockSize); - - /** - * @brief Initialization function for the Q7 sparse FIR filter. - * @param[in,out] *S points to an instance of the Q7 sparse FIR structure. - * @param[in] numTaps number of nonzero coefficients in the filter. - * @param[in] *pCoeffs points to the array of filter coefficients. - * @param[in] *pState points to the state buffer. - * @param[in] *pTapDelay points to the array of offset times. - * @param[in] maxDelay maximum offset time supported. - * @param[in] blockSize number of samples that will be processed per block. - * @return none - */ - - void arm_fir_sparse_init_q7( - arm_fir_sparse_instance_q7 * S, - uint16_t numTaps, - q7_t * pCoeffs, - q7_t * pState, - int32_t * pTapDelay, - uint16_t maxDelay, - uint32_t blockSize); - - - /* - * @brief Floating-point sin_cos function. - * @param[in] theta input value in degrees - * @param[out] *pSinVal points to the processed sine output. - * @param[out] *pCosVal points to the processed cos output. - * @return none. - */ - - void arm_sin_cos_f32( - float32_t theta, - float32_t * pSinVal, - float32_t * pCcosVal); - - /* - * @brief Q31 sin_cos function. - * @param[in] theta scaled input value in degrees - * @param[out] *pSinVal points to the processed sine output. - * @param[out] *pCosVal points to the processed cosine output. - * @return none. - */ - - void arm_sin_cos_q31( - q31_t theta, - q31_t * pSinVal, - q31_t * pCosVal); - - - /** - * @brief Floating-point complex conjugate. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] numSamples number of complex samples in each vector - * @return none. - */ - - void arm_cmplx_conj_f32( - float32_t * pSrc, - float32_t * pDst, - uint32_t numSamples); - - /** - * @brief Q31 complex conjugate. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] numSamples number of complex samples in each vector - * @return none. - */ - - void arm_cmplx_conj_q31( - q31_t * pSrc, - q31_t * pDst, - uint32_t numSamples); - - /** - * @brief Q15 complex conjugate. - * @param[in] *pSrc points to the input vector - * @param[out] *pDst points to the output vector - * @param[in] numSamples number of complex samples in each vector - * @return none. - */ - - void arm_cmplx_conj_q15( - q15_t * pSrc, - q15_t * pDst, - uint32_t numSamples); - - - - /** - * @brief Floating-point complex magnitude squared - * @param[in] *pSrc points to the complex input vector - * @param[out] *pDst points to the real output vector - * @param[in] numSamples number of complex samples in the input vector - * @return none. - */ - - void arm_cmplx_mag_squared_f32( - float32_t * pSrc, - float32_t * pDst, - uint32_t numSamples); - - /** - * @brief Q31 complex magnitude squared - * @param[in] *pSrc points to the complex input vector - * @param[out] *pDst points to the real output vector - * @param[in] numSamples number of complex samples in the input vector - * @return none. - */ - - void arm_cmplx_mag_squared_q31( - q31_t * pSrc, - q31_t * pDst, - uint32_t numSamples); - - /** - * @brief Q15 complex magnitude squared - * @param[in] *pSrc points to the complex input vector - * @param[out] *pDst points to the real output vector - * @param[in] numSamples number of complex samples in the input vector - * @return none. - */ - - void arm_cmplx_mag_squared_q15( - q15_t * pSrc, - q15_t * pDst, - uint32_t numSamples); - - - /** - * @ingroup groupController - */ - - /** - * @defgroup PID PID Motor Control - * - * A Proportional Integral Derivative (PID) controller is a generic feedback control - * loop mechanism widely used in industrial control systems. - * A PID controller is the most commonly used type of feedback controller. - * - * This set of functions implements (PID) controllers - * for Q15, Q31, and floating-point data types. The functions operate on a single sample - * of data and each call to the function returns a single processed value. - * S points to an instance of the PID control data structure. in - * is the input sample value. The functions return the output value. - * - * \par Algorithm: - * 
-   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
-   *    A0 = Kp + Ki + Kd
-   *    A1 = (-Kp ) - (2 * Kd )
-   *    A2 = Kd
- * - * \par - * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant - * - * \par - * \image html PID.gif "Proportional Integral Derivative Controller" - * - * \par - * The PID controller calculates an "error" value as the difference between - * the measured output and the reference input. - * The controller attempts to minimize the error by adjusting the process control inputs. - * The proportional value determines the reaction to the current error, - * the integral value determines the reaction based on the sum of recent errors, - * and the derivative value determines the reaction based on the rate at which the error has been changing. - * - * \par Instance Structure - * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure. - * A separate instance structure must be defined for each PID Controller. - * There are separate instance structure declarations for each of the 3 supported data types. - * - * \par Reset Functions - * There is also an associated reset function for each data type which clears the state array. - * - * \par Initialization Functions - * There is also an associated initialization function for each data type. - * The initialization function performs the following operations: - * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains. - * - Zeros out the values in the state buffer. - * - * \par - * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function. - * - * \par Fixed-Point Behavior - * Care must be taken when using the fixed-point versions of the PID Controller functions. - * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered. - * Refer to the function specific documentation below for usage guidelines. - */ - - /** - * @addtogroup PID - * @{ - */ - - /** - * @brief Process function for the floating-point PID Control. - * @param[in,out] *S is an instance of the floating-point PID Control structure - * @param[in] in input sample to process - * @return out processed output sample. - */ - - - static __INLINE float32_t arm_pid_f32( - arm_pid_instance_f32 * S, - float32_t in) - { - float32_t out; - - /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */ - out = (S->A0 * in) + - (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]); - - /* Update state */ - S->state[1] = S->state[0]; - S->state[0] = in; - S->state[2] = out; - - /* return to application */ - return (out); - - } - - /** - * @brief Process function for the Q31 PID Control. - * @param[in,out] *S points to an instance of the Q31 PID Control structure - * @param[in] in input sample to process - * @return out processed output sample. - * - * Scaling and Overflow Behavior: - * \par - * The function is implemented using an internal 64-bit accumulator. - * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit. - * Thus, if the accumulator result overflows it wraps around rather than clip. - * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions. - * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format. - */ - - static __INLINE q31_t arm_pid_q31( - arm_pid_instance_q31 * S, - q31_t in) - { - q63_t acc; - q31_t out; - - /* acc = A0 * x[n] */ - acc = (q63_t) S->A0 * in; - - /* acc += A1 * x[n-1] */ - acc += (q63_t) S->A1 * S->state[0]; - - /* acc += A2 * x[n-2] */ - acc += (q63_t) S->A2 * S->state[1]; - - /* convert output to 1.31 format to add y[n-1] */ - out = (q31_t) (acc >> 31u); - - /* out += y[n-1] */ - out += S->state[2]; - - /* Update state */ - S->state[1] = S->state[0]; - S->state[0] = in; - S->state[2] = out; - - /* return to application */ - return (out); - - } - - /** - * @brief Process function for the Q15 PID Control. - * @param[in,out] *S points to an instance of the Q15 PID Control structure - * @param[in] in input sample to process - * @return out processed output sample. - * - * Scaling and Overflow Behavior: - * \par - * The function is implemented using a 64-bit internal accumulator. - * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result. - * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format. - * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved. - * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits. - * Lastly, the accumulator is saturated to yield a result in 1.15 format. - */ - - static __INLINE q15_t arm_pid_q15( - arm_pid_instance_q15 * S, - q15_t in) - { - q63_t acc; - q15_t out; - -#ifndef ARM_MATH_CM0_FAMILY - __SIMD32_TYPE *vstate; - - /* Implementation of PID controller */ - - /* acc = A0 * x[n] */ - acc = (q31_t) __SMUAD(S->A0, in); - - /* acc += A1 * x[n-1] + A2 * x[n-2] */ - vstate = __SIMD32_CONST(S->state); - acc = __SMLALD(S->A1, (q31_t) *vstate, acc); - -#else - /* acc = A0 * x[n] */ - acc = ((q31_t) S->A0) * in; - - /* acc += A1 * x[n-1] + A2 * x[n-2] */ - acc += (q31_t) S->A1 * S->state[0]; - acc += (q31_t) S->A2 * S->state[1]; - -#endif - - /* acc += y[n-1] */ - acc += (q31_t) S->state[2] << 15; - - /* saturate the output */ - out = (q15_t) (__SSAT((acc >> 15), 16)); - - /* Update state */ - S->state[1] = S->state[0]; - S->state[0] = in; - S->state[2] = out; - - /* return to application */ - return (out); - - } - - /** - * @} end of PID group - */ - - - /** - * @brief Floating-point matrix inverse. - * @param[in] *src points to the instance of the input floating-point matrix structure. - * @param[out] *dst points to the instance of the output floating-point matrix structure. - * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match. - * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR. - */ - - arm_status arm_mat_inverse_f32( - const arm_matrix_instance_f32 * src, - arm_matrix_instance_f32 * dst); - - - - /** - * @ingroup groupController - */ - - - /** - * @defgroup clarke Vector Clarke Transform - * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector. - * Generally the Clarke transform uses three-phase currents Ia, Ib and Ic to calculate currents - * in the two-phase orthogonal stator axis Ialpha and Ibeta. - * When Ialpha is superposed with Ia as shown in the figure below - * \image html clarke.gif Stator current space vector and its components in (a,b). - * and Ia + Ib + Ic = 0, in this condition Ialpha and Ibeta - * can be calculated using only Ia and Ib. - * - * The function operates on a single sample of data and each call to the function returns the processed output. - * The library provides separate functions for Q31 and floating-point data types. - * \par Algorithm - * \image html clarkeFormula.gif - * where Ia and Ib are the instantaneous stator phases and - * pIalpha and pIbeta are the two coordinates of time invariant vector. - * \par Fixed-Point Behavior - * Care must be taken when using the Q31 version of the Clarke transform. - * In particular, the overflow and saturation behavior of the accumulator used must be considered. - * Refer to the function specific documentation below for usage guidelines. - */ - - /** - * @addtogroup clarke - * @{ - */ - - /** - * - * @brief Floating-point Clarke transform - * @param[in] Ia input three-phase coordinate a - * @param[in] Ib input three-phase coordinate b - * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha - * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta - * @return none. - */ - - static __INLINE void arm_clarke_f32( - float32_t Ia, - float32_t Ib, - float32_t * pIalpha, - float32_t * pIbeta) - { - /* Calculate pIalpha using the equation, pIalpha = Ia */ - *pIalpha = Ia; - - /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */ - *pIbeta = - ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib); - - } - - /** - * @brief Clarke transform for Q31 version - * @param[in] Ia input three-phase coordinate a - * @param[in] Ib input three-phase coordinate b - * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha - * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta - * @return none. - * - * Scaling and Overflow Behavior: - * \par - * The function is implemented using an internal 32-bit accumulator. - * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. - * There is saturation on the addition, hence there is no risk of overflow. - */ - - static __INLINE void arm_clarke_q31( - q31_t Ia, - q31_t Ib, - q31_t * pIalpha, - q31_t * pIbeta) - { - q31_t product1, product2; /* Temporary variables used to store intermediate results */ - - /* Calculating pIalpha from Ia by equation pIalpha = Ia */ - *pIalpha = Ia; - - /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */ - product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30); - - /* Intermediate product is calculated by (2/sqrt(3) * Ib) */ - product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30); - - /* pIbeta is calculated by adding the intermediate products */ - *pIbeta = __QADD(product1, product2); - } - - /** - * @} end of clarke group - */ - - /** - * @brief Converts the elements of the Q7 vector to Q31 vector. - * @param[in] *pSrc input pointer - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_q7_to_q31( - q7_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - - - - /** - * @ingroup groupController - */ - - /** - * @defgroup inv_clarke Vector Inverse Clarke Transform - * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases. - * - * The function operates on a single sample of data and each call to the function returns the processed output. - * The library provides separate functions for Q31 and floating-point data types. - * \par Algorithm - * \image html clarkeInvFormula.gif - * where pIa and pIb are the instantaneous stator phases and - * Ialpha and Ibeta are the two coordinates of time invariant vector. - * \par Fixed-Point Behavior - * Care must be taken when using the Q31 version of the Clarke transform. - * In particular, the overflow and saturation behavior of the accumulator used must be considered. - * Refer to the function specific documentation below for usage guidelines. - */ - - /** - * @addtogroup inv_clarke - * @{ - */ - - /** - * @brief Floating-point Inverse Clarke transform - * @param[in] Ialpha input two-phase orthogonal vector axis alpha - * @param[in] Ibeta input two-phase orthogonal vector axis beta - * @param[out] *pIa points to output three-phase coordinate a - * @param[out] *pIb points to output three-phase coordinate b - * @return none. - */ - - - static __INLINE void arm_inv_clarke_f32( - float32_t Ialpha, - float32_t Ibeta, - float32_t * pIa, - float32_t * pIb) - { - /* Calculating pIa from Ialpha by equation pIa = Ialpha */ - *pIa = Ialpha; - - /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */ - *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta; - - } - - /** - * @brief Inverse Clarke transform for Q31 version - * @param[in] Ialpha input two-phase orthogonal vector axis alpha - * @param[in] Ibeta input two-phase orthogonal vector axis beta - * @param[out] *pIa points to output three-phase coordinate a - * @param[out] *pIb points to output three-phase coordinate b - * @return none. - * - * Scaling and Overflow Behavior: - * \par - * The function is implemented using an internal 32-bit accumulator. - * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. - * There is saturation on the subtraction, hence there is no risk of overflow. - */ - - static __INLINE void arm_inv_clarke_q31( - q31_t Ialpha, - q31_t Ibeta, - q31_t * pIa, - q31_t * pIb) - { - q31_t product1, product2; /* Temporary variables used to store intermediate results */ - - /* Calculating pIa from Ialpha by equation pIa = Ialpha */ - *pIa = Ialpha; - - /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */ - product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31); - - /* Intermediate product is calculated by (1/sqrt(3) * pIb) */ - product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31); - - /* pIb is calculated by subtracting the products */ - *pIb = __QSUB(product2, product1); - - } - - /** - * @} end of inv_clarke group - */ - - /** - * @brief Converts the elements of the Q7 vector to Q15 vector. - * @param[in] *pSrc input pointer - * @param[out] *pDst output pointer - * @param[in] blockSize number of samples to process - * @return none. - */ - void arm_q7_to_q15( - q7_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - - - /** - * @ingroup groupController - */ - - /** - * @defgroup park Vector Park Transform - * - * Forward Park transform converts the input two-coordinate vector to flux and torque components. - * The Park transform can be used to realize the transformation of the Ialpha and the Ibeta currents - * from the stationary to the moving reference frame and control the spatial relationship between - * the stator vector current and rotor flux vector. - * If we consider the d axis aligned with the rotor flux, the diagram below shows the - * current vector and the relationship from the two reference frames: - * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame" - * - * The function operates on a single sample of data and each call to the function returns the processed output. - * The library provides separate functions for Q31 and floating-point data types. - * \par Algorithm - * \image html parkFormula.gif - * where Ialpha and Ibeta are the stator vector components, - * pId and pIq are rotor vector components and cosVal and sinVal are the - * cosine and sine values of theta (rotor flux position). - * \par Fixed-Point Behavior - * Care must be taken when using the Q31 version of the Park transform. - * In particular, the overflow and saturation behavior of the accumulator used must be considered. - * Refer to the function specific documentation below for usage guidelines. - */ - - /** - * @addtogroup park - * @{ - */ - - /** - * @brief Floating-point Park transform - * @param[in] Ialpha input two-phase vector coordinate alpha - * @param[in] Ibeta input two-phase vector coordinate beta - * @param[out] *pId points to output rotor reference frame d - * @param[out] *pIq points to output rotor reference frame q - * @param[in] sinVal sine value of rotation angle theta - * @param[in] cosVal cosine value of rotation angle theta - * @return none. - * - * The function implements the forward Park transform. - * - */ - - static __INLINE void arm_park_f32( - float32_t Ialpha, - float32_t Ibeta, - float32_t * pId, - float32_t * pIq, - float32_t sinVal, - float32_t cosVal) - { - /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */ - *pId = Ialpha * cosVal + Ibeta * sinVal; - - /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */ - *pIq = -Ialpha * sinVal + Ibeta * cosVal; - - } - - /** - * @brief Park transform for Q31 version - * @param[in] Ialpha input two-phase vector coordinate alpha - * @param[in] Ibeta input two-phase vector coordinate beta - * @param[out] *pId points to output rotor reference frame d - * @param[out] *pIq points to output rotor reference frame q - * @param[in] sinVal sine value of rotation angle theta - * @param[in] cosVal cosine value of rotation angle theta - * @return none. - * - * Scaling and Overflow Behavior: - * \par - * The function is implemented using an internal 32-bit accumulator. - * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. - * There is saturation on the addition and subtraction, hence there is no risk of overflow. - */ - - - static __INLINE void arm_park_q31( - q31_t Ialpha, - q31_t Ibeta, - q31_t * pId, - q31_t * pIq, - q31_t sinVal, - q31_t cosVal) - { - q31_t product1, product2; /* Temporary variables used to store intermediate results */ - q31_t product3, product4; /* Temporary variables used to store intermediate results */ - - /* Intermediate product is calculated by (Ialpha * cosVal) */ - product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31); - - /* Intermediate product is calculated by (Ibeta * sinVal) */ - product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31); - - - /* Intermediate product is calculated by (Ialpha * sinVal) */ - product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31); - - /* Intermediate product is calculated by (Ibeta * cosVal) */ - product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31); - - /* Calculate pId by adding the two intermediate products 1 and 2 */ - *pId = __QADD(product1, product2); - - /* Calculate pIq by subtracting the two intermediate products 3 from 4 */ - *pIq = __QSUB(product4, product3); - } - - /** - * @} end of park group - */ - - /** - * @brief Converts the elements of the Q7 vector to floating-point vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q7_to_float( - q7_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - - /** - * @ingroup groupController - */ - - /** - * @defgroup inv_park Vector Inverse Park transform - * Inverse Park transform converts the input flux and torque components to two-coordinate vector. - * - * The function operates on a single sample of data and each call to the function returns the processed output. - * The library provides separate functions for Q31 and floating-point data types. - * \par Algorithm - * \image html parkInvFormula.gif - * where pIalpha and pIbeta are the stator vector components, - * Id and Iq are rotor vector components and cosVal and sinVal are the - * cosine and sine values of theta (rotor flux position). - * \par Fixed-Point Behavior - * Care must be taken when using the Q31 version of the Park transform. - * In particular, the overflow and saturation behavior of the accumulator used must be considered. - * Refer to the function specific documentation below for usage guidelines. - */ - - /** - * @addtogroup inv_park - * @{ - */ - - /** - * @brief Floating-point Inverse Park transform - * @param[in] Id input coordinate of rotor reference frame d - * @param[in] Iq input coordinate of rotor reference frame q - * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha - * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta - * @param[in] sinVal sine value of rotation angle theta - * @param[in] cosVal cosine value of rotation angle theta - * @return none. - */ - - static __INLINE void arm_inv_park_f32( - float32_t Id, - float32_t Iq, - float32_t * pIalpha, - float32_t * pIbeta, - float32_t sinVal, - float32_t cosVal) - { - /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */ - *pIalpha = Id * cosVal - Iq * sinVal; - - /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */ - *pIbeta = Id * sinVal + Iq * cosVal; - - } - - - /** - * @brief Inverse Park transform for Q31 version - * @param[in] Id input coordinate of rotor reference frame d - * @param[in] Iq input coordinate of rotor reference frame q - * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha - * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta - * @param[in] sinVal sine value of rotation angle theta - * @param[in] cosVal cosine value of rotation angle theta - * @return none. - * - * Scaling and Overflow Behavior: - * \par - * The function is implemented using an internal 32-bit accumulator. - * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format. - * There is saturation on the addition, hence there is no risk of overflow. - */ - - - static __INLINE void arm_inv_park_q31( - q31_t Id, - q31_t Iq, - q31_t * pIalpha, - q31_t * pIbeta, - q31_t sinVal, - q31_t cosVal) - { - q31_t product1, product2; /* Temporary variables used to store intermediate results */ - q31_t product3, product4; /* Temporary variables used to store intermediate results */ - - /* Intermediate product is calculated by (Id * cosVal) */ - product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31); - - /* Intermediate product is calculated by (Iq * sinVal) */ - product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31); - - - /* Intermediate product is calculated by (Id * sinVal) */ - product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31); - - /* Intermediate product is calculated by (Iq * cosVal) */ - product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31); - - /* Calculate pIalpha by using the two intermediate products 1 and 2 */ - *pIalpha = __QSUB(product1, product2); - - /* Calculate pIbeta by using the two intermediate products 3 and 4 */ - *pIbeta = __QADD(product4, product3); - - } - - /** - * @} end of Inverse park group - */ - - - /** - * @brief Converts the elements of the Q31 vector to floating-point vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q31_to_float( - q31_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - /** - * @ingroup groupInterpolation - */ - - /** - * @defgroup LinearInterpolate Linear Interpolation - * - * Linear interpolation is a method of curve fitting using linear polynomials. - * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line - * - * \par - * \image html LinearInterp.gif "Linear interpolation" - * - * \par - * A Linear Interpolate function calculates an output value(y), for the input(x) - * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values) - * - * \par Algorithm: - * 
-   *       y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
-   *       where x0, x1 are nearest values of input x
-   *             y0, y1 are nearest values to output y
-   *
- * - * \par - * This set of functions implements Linear interpolation process - * for Q7, Q15, Q31, and floating-point data types. The functions operate on a single - * sample of data and each call to the function returns a single processed value. - * S points to an instance of the Linear Interpolate function data structure. - * x is the input sample value. The functions returns the output value. - * - * \par - * if x is outside of the table boundary, Linear interpolation returns first value of the table - * if x is below input range and returns last value of table if x is above range. - */ - - /** - * @addtogroup LinearInterpolate - * @{ - */ - - /** - * @brief Process function for the floating-point Linear Interpolation Function. - * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure - * @param[in] x input sample to process - * @return y processed output sample. - * - */ - - static __INLINE float32_t arm_linear_interp_f32( - arm_linear_interp_instance_f32 * S, - float32_t x) - { - - float32_t y; - float32_t x0, x1; /* Nearest input values */ - float32_t y0, y1; /* Nearest output values */ - float32_t xSpacing = S->xSpacing; /* spacing between input values */ - int32_t i; /* Index variable */ - float32_t *pYData = S->pYData; /* pointer to output table */ - - /* Calculation of index */ - i = (int32_t) ((x - S->x1) / xSpacing); - - if(i < 0) - { - /* Iniatilize output for below specified range as least output value of table */ - y = pYData[0]; - } - else if((uint32_t)i >= S->nValues) - { - /* Iniatilize output for above specified range as last output value of table */ - y = pYData[S->nValues - 1]; - } - else - { - /* Calculation of nearest input values */ - x0 = S->x1 + i * xSpacing; - x1 = S->x1 + (i + 1) * xSpacing; - - /* Read of nearest output values */ - y0 = pYData[i]; - y1 = pYData[i + 1]; - - /* Calculation of output */ - y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0)); - - } - - /* returns output value */ - return (y); - } - - /** - * - * @brief Process function for the Q31 Linear Interpolation Function. - * @param[in] *pYData pointer to Q31 Linear Interpolation table - * @param[in] x input sample to process - * @param[in] nValues number of table values - * @return y processed output sample. - * - * \par - * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. - * This function can support maximum of table size 2^12. - * - */ - - - static __INLINE q31_t arm_linear_interp_q31( - q31_t * pYData, - q31_t x, - uint32_t nValues) - { - q31_t y; /* output */ - q31_t y0, y1; /* Nearest output values */ - q31_t fract; /* fractional part */ - int32_t index; /* Index to read nearest output values */ - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - index = ((x & 0xFFF00000) >> 20); - - if(index >= (int32_t)(nValues - 1)) - { - return (pYData[nValues - 1]); - } - else if(index < 0) - { - return (pYData[0]); - } - else - { - - /* 20 bits for the fractional part */ - /* shift left by 11 to keep fract in 1.31 format */ - fract = (x & 0x000FFFFF) << 11; - - /* Read two nearest output values from the index in 1.31(q31) format */ - y0 = pYData[index]; - y1 = pYData[index + 1u]; - - /* Calculation of y0 * (1-fract) and y is in 2.30 format */ - y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32)); - - /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */ - y += ((q31_t) (((q63_t) y1 * fract) >> 32)); - - /* Convert y to 1.31 format */ - return (y << 1u); - - } - - } - - /** - * - * @brief Process function for the Q15 Linear Interpolation Function. - * @param[in] *pYData pointer to Q15 Linear Interpolation table - * @param[in] x input sample to process - * @param[in] nValues number of table values - * @return y processed output sample. - * - * \par - * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. - * This function can support maximum of table size 2^12. - * - */ - - - static __INLINE q15_t arm_linear_interp_q15( - q15_t * pYData, - q31_t x, - uint32_t nValues) - { - q63_t y; /* output */ - q15_t y0, y1; /* Nearest output values */ - q31_t fract; /* fractional part */ - int32_t index; /* Index to read nearest output values */ - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - index = ((x & 0xFFF00000) >> 20u); - - if(index >= (int32_t)(nValues - 1)) - { - return (pYData[nValues - 1]); - } - else if(index < 0) - { - return (pYData[0]); - } - else - { - /* 20 bits for the fractional part */ - /* fract is in 12.20 format */ - fract = (x & 0x000FFFFF); - - /* Read two nearest output values from the index */ - y0 = pYData[index]; - y1 = pYData[index + 1u]; - - /* Calculation of y0 * (1-fract) and y is in 13.35 format */ - y = ((q63_t) y0 * (0xFFFFF - fract)); - - /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */ - y += ((q63_t) y1 * (fract)); - - /* convert y to 1.15 format */ - return (y >> 20); - } - - - } - - /** - * - * @brief Process function for the Q7 Linear Interpolation Function. - * @param[in] *pYData pointer to Q7 Linear Interpolation table - * @param[in] x input sample to process - * @param[in] nValues number of table values - * @return y processed output sample. - * - * \par - * Input sample x is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part. - * This function can support maximum of table size 2^12. - */ - - - static __INLINE q7_t arm_linear_interp_q7( - q7_t * pYData, - q31_t x, - uint32_t nValues) - { - q31_t y; /* output */ - q7_t y0, y1; /* Nearest output values */ - q31_t fract; /* fractional part */ - uint32_t index; /* Index to read nearest output values */ - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - if (x < 0) - { - return (pYData[0]); - } - index = (x >> 20) & 0xfff; - - - if(index >= (nValues - 1)) - { - return (pYData[nValues - 1]); - } - else - { - - /* 20 bits for the fractional part */ - /* fract is in 12.20 format */ - fract = (x & 0x000FFFFF); - - /* Read two nearest output values from the index and are in 1.7(q7) format */ - y0 = pYData[index]; - y1 = pYData[index + 1u]; - - /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */ - y = ((y0 * (0xFFFFF - fract))); - - /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */ - y += (y1 * fract); - - /* convert y to 1.7(q7) format */ - return (y >> 20u); - - } - - } - /** - * @} end of LinearInterpolate group - */ - - /** - * @brief Fast approximation to the trigonometric sine function for floating-point data. - * @param[in] x input value in radians. - * @return sin(x). - */ - - float32_t arm_sin_f32( - float32_t x); - - /** - * @brief Fast approximation to the trigonometric sine function for Q31 data. - * @param[in] x Scaled input value in radians. - * @return sin(x). - */ - - q31_t arm_sin_q31( - q31_t x); - - /** - * @brief Fast approximation to the trigonometric sine function for Q15 data. - * @param[in] x Scaled input value in radians. - * @return sin(x). - */ - - q15_t arm_sin_q15( - q15_t x); - - /** - * @brief Fast approximation to the trigonometric cosine function for floating-point data. - * @param[in] x input value in radians. - * @return cos(x). - */ - - float32_t arm_cos_f32( - float32_t x); - - /** - * @brief Fast approximation to the trigonometric cosine function for Q31 data. - * @param[in] x Scaled input value in radians. - * @return cos(x). - */ - - q31_t arm_cos_q31( - q31_t x); - - /** - * @brief Fast approximation to the trigonometric cosine function for Q15 data. - * @param[in] x Scaled input value in radians. - * @return cos(x). - */ - - q15_t arm_cos_q15( - q15_t x); - - - /** - * @ingroup groupFastMath - */ - - - /** - * @defgroup SQRT Square Root - * - * Computes the square root of a number. - * There are separate functions for Q15, Q31, and floating-point data types. - * The square root function is computed using the Newton-Raphson algorithm. - * This is an iterative algorithm of the form: - * 
-   *      x1 = x0 - f(x0)/f'(x0)
-   *
- * where x1 is the current estimate, - * x0 is the previous estimate, and - * f'(x0) is the derivative of f() evaluated at x0. - * For the square root function, the algorithm reduces to: - * 
-   *     x0 = in/2                         [initial guess]
-   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]
-   *
- */ - - - /** - * @addtogroup SQRT - * @{ - */ - - /** - * @brief Floating-point square root function. - * @param[in] in input value. - * @param[out] *pOut square root of input value. - * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if - * in is negative value and returns zero output for negative values. - */ - - static __INLINE arm_status arm_sqrt_f32( - float32_t in, - float32_t * pOut) - { - if(in > 0) - { - -// #if __FPU_USED -#if (__FPU_USED == 1) && defined ( __CC_ARM ) - *pOut = __sqrtf(in); -#else - *pOut = sqrtf(in); -#endif - - return (ARM_MATH_SUCCESS); - } - else - { - *pOut = 0.0f; - return (ARM_MATH_ARGUMENT_ERROR); - } - - } - - - /** - * @brief Q31 square root function. - * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF. - * @param[out] *pOut square root of input value. - * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if - * in is negative value and returns zero output for negative values. - */ - arm_status arm_sqrt_q31( - q31_t in, - q31_t * pOut); - - /** - * @brief Q15 square root function. - * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF. - * @param[out] *pOut square root of input value. - * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if - * in is negative value and returns zero output for negative values. - */ - arm_status arm_sqrt_q15( - q15_t in, - q15_t * pOut); - - /** - * @} end of SQRT group - */ - - - - - - - /** - * @brief floating-point Circular write function. - */ - - static __INLINE void arm_circularWrite_f32( - int32_t * circBuffer, - int32_t L, - uint16_t * writeOffset, - int32_t bufferInc, - const int32_t * src, - int32_t srcInc, - uint32_t blockSize) - { - uint32_t i = 0u; - int32_t wOffset; - - /* Copy the value of Index pointer that points - * to the current location where the input samples to be copied */ - wOffset = *writeOffset; - - /* Loop over the blockSize */ - i = blockSize; - - while(i > 0u) - { - /* copy the input sample to the circular buffer */ - circBuffer[wOffset] = *src; - - /* Update the input pointer */ - src += srcInc; - - /* Circularly update wOffset. Watch out for positive and negative value */ - wOffset += bufferInc; - if(wOffset >= L) - wOffset -= L; - - /* Decrement the loop counter */ - i--; - } - - /* Update the index pointer */ - *writeOffset = wOffset; - } - - - - /** - * @brief floating-point Circular Read function. - */ - static __INLINE void arm_circularRead_f32( - int32_t * circBuffer, - int32_t L, - int32_t * readOffset, - int32_t bufferInc, - int32_t * dst, - int32_t * dst_base, - int32_t dst_length, - int32_t dstInc, - uint32_t blockSize) - { - uint32_t i = 0u; - int32_t rOffset, dst_end; - - /* Copy the value of Index pointer that points - * to the current location from where the input samples to be read */ - rOffset = *readOffset; - dst_end = (int32_t) (dst_base + dst_length); - - /* Loop over the blockSize */ - i = blockSize; - - while(i > 0u) - { - /* copy the sample from the circular buffer to the destination buffer */ - *dst = circBuffer[rOffset]; - - /* Update the input pointer */ - dst += dstInc; - - if(dst == (int32_t *) dst_end) - { - dst = dst_base; - } - - /* Circularly update rOffset. Watch out for positive and negative value */ - rOffset += bufferInc; - - if(rOffset >= L) - { - rOffset -= L; - } - - /* Decrement the loop counter */ - i--; - } - - /* Update the index pointer */ - *readOffset = rOffset; - } - - /** - * @brief Q15 Circular write function. - */ - - static __INLINE void arm_circularWrite_q15( - q15_t * circBuffer, - int32_t L, - uint16_t * writeOffset, - int32_t bufferInc, - const q15_t * src, - int32_t srcInc, - uint32_t blockSize) - { - uint32_t i = 0u; - int32_t wOffset; - - /* Copy the value of Index pointer that points - * to the current location where the input samples to be copied */ - wOffset = *writeOffset; - - /* Loop over the blockSize */ - i = blockSize; - - while(i > 0u) - { - /* copy the input sample to the circular buffer */ - circBuffer[wOffset] = *src; - - /* Update the input pointer */ - src += srcInc; - - /* Circularly update wOffset. Watch out for positive and negative value */ - wOffset += bufferInc; - if(wOffset >= L) - wOffset -= L; - - /* Decrement the loop counter */ - i--; - } - - /* Update the index pointer */ - *writeOffset = wOffset; - } - - - - /** - * @brief Q15 Circular Read function. - */ - static __INLINE void arm_circularRead_q15( - q15_t * circBuffer, - int32_t L, - int32_t * readOffset, - int32_t bufferInc, - q15_t * dst, - q15_t * dst_base, - int32_t dst_length, - int32_t dstInc, - uint32_t blockSize) - { - uint32_t i = 0; - int32_t rOffset, dst_end; - - /* Copy the value of Index pointer that points - * to the current location from where the input samples to be read */ - rOffset = *readOffset; - - dst_end = (int32_t) (dst_base + dst_length); - - /* Loop over the blockSize */ - i = blockSize; - - while(i > 0u) - { - /* copy the sample from the circular buffer to the destination buffer */ - *dst = circBuffer[rOffset]; - - /* Update the input pointer */ - dst += dstInc; - - if(dst == (q15_t *) dst_end) - { - dst = dst_base; - } - - /* Circularly update wOffset. Watch out for positive and negative value */ - rOffset += bufferInc; - - if(rOffset >= L) - { - rOffset -= L; - } - - /* Decrement the loop counter */ - i--; - } - - /* Update the index pointer */ - *readOffset = rOffset; - } - - - /** - * @brief Q7 Circular write function. - */ - - static __INLINE void arm_circularWrite_q7( - q7_t * circBuffer, - int32_t L, - uint16_t * writeOffset, - int32_t bufferInc, - const q7_t * src, - int32_t srcInc, - uint32_t blockSize) - { - uint32_t i = 0u; - int32_t wOffset; - - /* Copy the value of Index pointer that points - * to the current location where the input samples to be copied */ - wOffset = *writeOffset; - - /* Loop over the blockSize */ - i = blockSize; - - while(i > 0u) - { - /* copy the input sample to the circular buffer */ - circBuffer[wOffset] = *src; - - /* Update the input pointer */ - src += srcInc; - - /* Circularly update wOffset. Watch out for positive and negative value */ - wOffset += bufferInc; - if(wOffset >= L) - wOffset -= L; - - /* Decrement the loop counter */ - i--; - } - - /* Update the index pointer */ - *writeOffset = wOffset; - } - - - - /** - * @brief Q7 Circular Read function. - */ - static __INLINE void arm_circularRead_q7( - q7_t * circBuffer, - int32_t L, - int32_t * readOffset, - int32_t bufferInc, - q7_t * dst, - q7_t * dst_base, - int32_t dst_length, - int32_t dstInc, - uint32_t blockSize) - { - uint32_t i = 0; - int32_t rOffset, dst_end; - - /* Copy the value of Index pointer that points - * to the current location from where the input samples to be read */ - rOffset = *readOffset; - - dst_end = (int32_t) (dst_base + dst_length); - - /* Loop over the blockSize */ - i = blockSize; - - while(i > 0u) - { - /* copy the sample from the circular buffer to the destination buffer */ - *dst = circBuffer[rOffset]; - - /* Update the input pointer */ - dst += dstInc; - - if(dst == (q7_t *) dst_end) - { - dst = dst_base; - } - - /* Circularly update rOffset. Watch out for positive and negative value */ - rOffset += bufferInc; - - if(rOffset >= L) - { - rOffset -= L; - } - - /* Decrement the loop counter */ - i--; - } - - /* Update the index pointer */ - *readOffset = rOffset; - } - - - /** - * @brief Sum of the squares of the elements of a Q31 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_power_q31( - q31_t * pSrc, - uint32_t blockSize, - q63_t * pResult); - - /** - * @brief Sum of the squares of the elements of a floating-point vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_power_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult); - - /** - * @brief Sum of the squares of the elements of a Q15 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_power_q15( - q15_t * pSrc, - uint32_t blockSize, - q63_t * pResult); - - /** - * @brief Sum of the squares of the elements of a Q7 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_power_q7( - q7_t * pSrc, - uint32_t blockSize, - q31_t * pResult); - - /** - * @brief Mean value of a Q7 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_mean_q7( - q7_t * pSrc, - uint32_t blockSize, - q7_t * pResult); - - /** - * @brief Mean value of a Q15 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - void arm_mean_q15( - q15_t * pSrc, - uint32_t blockSize, - q15_t * pResult); - - /** - * @brief Mean value of a Q31 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - void arm_mean_q31( - q31_t * pSrc, - uint32_t blockSize, - q31_t * pResult); - - /** - * @brief Mean value of a floating-point vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - void arm_mean_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult); - - /** - * @brief Variance of the elements of a floating-point vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_var_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult); - - /** - * @brief Variance of the elements of a Q31 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_var_q31( - q31_t * pSrc, - uint32_t blockSize, - q63_t * pResult); - - /** - * @brief Variance of the elements of a Q15 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_var_q15( - q15_t * pSrc, - uint32_t blockSize, - q31_t * pResult); - - /** - * @brief Root Mean Square of the elements of a floating-point vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_rms_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult); - - /** - * @brief Root Mean Square of the elements of a Q31 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_rms_q31( - q31_t * pSrc, - uint32_t blockSize, - q31_t * pResult); - - /** - * @brief Root Mean Square of the elements of a Q15 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_rms_q15( - q15_t * pSrc, - uint32_t blockSize, - q15_t * pResult); - - /** - * @brief Standard deviation of the elements of a floating-point vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_std_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult); - - /** - * @brief Standard deviation of the elements of a Q31 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_std_q31( - q31_t * pSrc, - uint32_t blockSize, - q31_t * pResult); - - /** - * @brief Standard deviation of the elements of a Q15 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output value. - * @return none. - */ - - void arm_std_q15( - q15_t * pSrc, - uint32_t blockSize, - q15_t * pResult); - - /** - * @brief Floating-point complex magnitude - * @param[in] *pSrc points to the complex input vector - * @param[out] *pDst points to the real output vector - * @param[in] numSamples number of complex samples in the input vector - * @return none. - */ - - void arm_cmplx_mag_f32( - float32_t * pSrc, - float32_t * pDst, - uint32_t numSamples); - - /** - * @brief Q31 complex magnitude - * @param[in] *pSrc points to the complex input vector - * @param[out] *pDst points to the real output vector - * @param[in] numSamples number of complex samples in the input vector - * @return none. - */ - - void arm_cmplx_mag_q31( - q31_t * pSrc, - q31_t * pDst, - uint32_t numSamples); - - /** - * @brief Q15 complex magnitude - * @param[in] *pSrc points to the complex input vector - * @param[out] *pDst points to the real output vector - * @param[in] numSamples number of complex samples in the input vector - * @return none. - */ - - void arm_cmplx_mag_q15( - q15_t * pSrc, - q15_t * pDst, - uint32_t numSamples); - - /** - * @brief Q15 complex dot product - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] numSamples number of complex samples in each vector - * @param[out] *realResult real part of the result returned here - * @param[out] *imagResult imaginary part of the result returned here - * @return none. - */ - - void arm_cmplx_dot_prod_q15( - q15_t * pSrcA, - q15_t * pSrcB, - uint32_t numSamples, - q31_t * realResult, - q31_t * imagResult); - - /** - * @brief Q31 complex dot product - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] numSamples number of complex samples in each vector - * @param[out] *realResult real part of the result returned here - * @param[out] *imagResult imaginary part of the result returned here - * @return none. - */ - - void arm_cmplx_dot_prod_q31( - q31_t * pSrcA, - q31_t * pSrcB, - uint32_t numSamples, - q63_t * realResult, - q63_t * imagResult); - - /** - * @brief Floating-point complex dot product - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[in] numSamples number of complex samples in each vector - * @param[out] *realResult real part of the result returned here - * @param[out] *imagResult imaginary part of the result returned here - * @return none. - */ - - void arm_cmplx_dot_prod_f32( - float32_t * pSrcA, - float32_t * pSrcB, - uint32_t numSamples, - float32_t * realResult, - float32_t * imagResult); - - /** - * @brief Q15 complex-by-real multiplication - * @param[in] *pSrcCmplx points to the complex input vector - * @param[in] *pSrcReal points to the real input vector - * @param[out] *pCmplxDst points to the complex output vector - * @param[in] numSamples number of samples in each vector - * @return none. - */ - - void arm_cmplx_mult_real_q15( - q15_t * pSrcCmplx, - q15_t * pSrcReal, - q15_t * pCmplxDst, - uint32_t numSamples); - - /** - * @brief Q31 complex-by-real multiplication - * @param[in] *pSrcCmplx points to the complex input vector - * @param[in] *pSrcReal points to the real input vector - * @param[out] *pCmplxDst points to the complex output vector - * @param[in] numSamples number of samples in each vector - * @return none. - */ - - void arm_cmplx_mult_real_q31( - q31_t * pSrcCmplx, - q31_t * pSrcReal, - q31_t * pCmplxDst, - uint32_t numSamples); - - /** - * @brief Floating-point complex-by-real multiplication - * @param[in] *pSrcCmplx points to the complex input vector - * @param[in] *pSrcReal points to the real input vector - * @param[out] *pCmplxDst points to the complex output vector - * @param[in] numSamples number of samples in each vector - * @return none. - */ - - void arm_cmplx_mult_real_f32( - float32_t * pSrcCmplx, - float32_t * pSrcReal, - float32_t * pCmplxDst, - uint32_t numSamples); - - /** - * @brief Minimum value of a Q7 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *result is output pointer - * @param[in] index is the array index of the minimum value in the input buffer. - * @return none. - */ - - void arm_min_q7( - q7_t * pSrc, - uint32_t blockSize, - q7_t * result, - uint32_t * index); - - /** - * @brief Minimum value of a Q15 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output pointer - * @param[in] *pIndex is the array index of the minimum value in the input buffer. - * @return none. - */ - - void arm_min_q15( - q15_t * pSrc, - uint32_t blockSize, - q15_t * pResult, - uint32_t * pIndex); - - /** - * @brief Minimum value of a Q31 vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output pointer - * @param[out] *pIndex is the array index of the minimum value in the input buffer. - * @return none. - */ - void arm_min_q31( - q31_t * pSrc, - uint32_t blockSize, - q31_t * pResult, - uint32_t * pIndex); - - /** - * @brief Minimum value of a floating-point vector. - * @param[in] *pSrc is input pointer - * @param[in] blockSize is the number of samples to process - * @param[out] *pResult is output pointer - * @param[out] *pIndex is the array index of the minimum value in the input buffer. - * @return none. - */ - - void arm_min_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult, - uint32_t * pIndex); - -/** - * @brief Maximum value of a Q7 vector. - * @param[in] *pSrc points to the input buffer - * @param[in] blockSize length of the input vector - * @param[out] *pResult maximum value returned here - * @param[out] *pIndex index of maximum value returned here - * @return none. - */ - - void arm_max_q7( - q7_t * pSrc, - uint32_t blockSize, - q7_t * pResult, - uint32_t * pIndex); - -/** - * @brief Maximum value of a Q15 vector. - * @param[in] *pSrc points to the input buffer - * @param[in] blockSize length of the input vector - * @param[out] *pResult maximum value returned here - * @param[out] *pIndex index of maximum value returned here - * @return none. - */ - - void arm_max_q15( - q15_t * pSrc, - uint32_t blockSize, - q15_t * pResult, - uint32_t * pIndex); - -/** - * @brief Maximum value of a Q31 vector. - * @param[in] *pSrc points to the input buffer - * @param[in] blockSize length of the input vector - * @param[out] *pResult maximum value returned here - * @param[out] *pIndex index of maximum value returned here - * @return none. - */ - - void arm_max_q31( - q31_t * pSrc, - uint32_t blockSize, - q31_t * pResult, - uint32_t * pIndex); - -/** - * @brief Maximum value of a floating-point vector. - * @param[in] *pSrc points to the input buffer - * @param[in] blockSize length of the input vector - * @param[out] *pResult maximum value returned here - * @param[out] *pIndex index of maximum value returned here - * @return none. - */ - - void arm_max_f32( - float32_t * pSrc, - uint32_t blockSize, - float32_t * pResult, - uint32_t * pIndex); - - /** - * @brief Q15 complex-by-complex multiplication - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] numSamples number of complex samples in each vector - * @return none. - */ - - void arm_cmplx_mult_cmplx_q15( - q15_t * pSrcA, - q15_t * pSrcB, - q15_t * pDst, - uint32_t numSamples); - - /** - * @brief Q31 complex-by-complex multiplication - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] numSamples number of complex samples in each vector - * @return none. - */ - - void arm_cmplx_mult_cmplx_q31( - q31_t * pSrcA, - q31_t * pSrcB, - q31_t * pDst, - uint32_t numSamples); - - /** - * @brief Floating-point complex-by-complex multiplication - * @param[in] *pSrcA points to the first input vector - * @param[in] *pSrcB points to the second input vector - * @param[out] *pDst points to the output vector - * @param[in] numSamples number of complex samples in each vector - * @return none. - */ - - void arm_cmplx_mult_cmplx_f32( - float32_t * pSrcA, - float32_t * pSrcB, - float32_t * pDst, - uint32_t numSamples); - - /** - * @brief Converts the elements of the floating-point vector to Q31 vector. - * @param[in] *pSrc points to the floating-point input vector - * @param[out] *pDst points to the Q31 output vector - * @param[in] blockSize length of the input vector - * @return none. - */ - void arm_float_to_q31( - float32_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - /** - * @brief Converts the elements of the floating-point vector to Q15 vector. - * @param[in] *pSrc points to the floating-point input vector - * @param[out] *pDst points to the Q15 output vector - * @param[in] blockSize length of the input vector - * @return none - */ - void arm_float_to_q15( - float32_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Converts the elements of the floating-point vector to Q7 vector. - * @param[in] *pSrc points to the floating-point input vector - * @param[out] *pDst points to the Q7 output vector - * @param[in] blockSize length of the input vector - * @return none - */ - void arm_float_to_q7( - float32_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - - /** - * @brief Converts the elements of the Q31 vector to Q15 vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q31_to_q15( - q31_t * pSrc, - q15_t * pDst, - uint32_t blockSize); - - /** - * @brief Converts the elements of the Q31 vector to Q7 vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q31_to_q7( - q31_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - /** - * @brief Converts the elements of the Q15 vector to floating-point vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q15_to_float( - q15_t * pSrc, - float32_t * pDst, - uint32_t blockSize); - - - /** - * @brief Converts the elements of the Q15 vector to Q31 vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q15_to_q31( - q15_t * pSrc, - q31_t * pDst, - uint32_t blockSize); - - - /** - * @brief Converts the elements of the Q15 vector to Q7 vector. - * @param[in] *pSrc is input pointer - * @param[out] *pDst is output pointer - * @param[in] blockSize is the number of samples to process - * @return none. - */ - void arm_q15_to_q7( - q15_t * pSrc, - q7_t * pDst, - uint32_t blockSize); - - - /** - * @ingroup groupInterpolation - */ - - /** - * @defgroup BilinearInterpolate Bilinear Interpolation - * - * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid. - * The underlying function f(x, y) is sampled on a regular grid and the interpolation process - * determines values between the grid points. - * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension. - * Bilinear interpolation is often used in image processing to rescale images. - * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types. - * - * Algorithm - * \par - * The instance structure used by the bilinear interpolation functions describes a two dimensional data table. - * For floating-point, the instance structure is defined as: - * 
-   *   typedef struct
-   *   {
-   *     uint16_t numRows;
-   *     uint16_t numCols;
-   *     float32_t *pData;
-   * } arm_bilinear_interp_instance_f32;
-   *
- * - * \par - * where numRows specifies the number of rows in the table; - * numCols specifies the number of columns in the table; - * and pData points to an array of size numRows*numCols values. - * The data table pTable is organized in row order and the supplied data values fall on integer indexes. - * That is, table element (x,y) is located at pTable[x + y*numCols] where x and y are integers. - * - * \par - * Let (x, y) specify the desired interpolation point. Then define: - * 
-   *     XF = floor(x)
-   *     YF = floor(y)
-   *
- * \par - * The interpolated output point is computed as: - * 
-   *  f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
-   *           + f(XF+1, YF) * (x-XF)*(1-(y-YF))
-   *           + f(XF, YF+1) * (1-(x-XF))*(y-YF)
-   *           + f(XF+1, YF+1) * (x-XF)*(y-YF)
-   *
- * Note that the coordinates (x, y) contain integer and fractional components. - * The integer components specify which portion of the table to use while the - * fractional components control the interpolation processor. - * - * \par - * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output. - */ - - /** - * @addtogroup BilinearInterpolate - * @{ - */ - - /** - * - * @brief Floating-point bilinear interpolation. - * @param[in,out] *S points to an instance of the interpolation structure. - * @param[in] X interpolation coordinate. - * @param[in] Y interpolation coordinate. - * @return out interpolated value. - */ - - - static __INLINE float32_t arm_bilinear_interp_f32( - const arm_bilinear_interp_instance_f32 * S, - float32_t X, - float32_t Y) - { - float32_t out; - float32_t f00, f01, f10, f11; - float32_t *pData = S->pData; - int32_t xIndex, yIndex, index; - float32_t xdiff, ydiff; - float32_t b1, b2, b3, b4; - - xIndex = (int32_t) X; - yIndex = (int32_t) Y; - - /* Care taken for table outside boundary */ - /* Returns zero output when values are outside table boundary */ - if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0 - || yIndex > (S->numCols - 1)) - { - return (0); - } - - /* Calculation of index for two nearest points in X-direction */ - index = (xIndex - 1) + (yIndex - 1) * S->numCols; - - - /* Read two nearest points in X-direction */ - f00 = pData[index]; - f01 = pData[index + 1]; - - /* Calculation of index for two nearest points in Y-direction */ - index = (xIndex - 1) + (yIndex) * S->numCols; - - - /* Read two nearest points in Y-direction */ - f10 = pData[index]; - f11 = pData[index + 1]; - - /* Calculation of intermediate values */ - b1 = f00; - b2 = f01 - f00; - b3 = f10 - f00; - b4 = f00 - f01 - f10 + f11; - - /* Calculation of fractional part in X */ - xdiff = X - xIndex; - - /* Calculation of fractional part in Y */ - ydiff = Y - yIndex; - - /* Calculation of bi-linear interpolated output */ - out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff; - - /* return to application */ - return (out); - - } - - /** - * - * @brief Q31 bilinear interpolation. - * @param[in,out] *S points to an instance of the interpolation structure. - * @param[in] X interpolation coordinate in 12.20 format. - * @param[in] Y interpolation coordinate in 12.20 format. - * @return out interpolated value. - */ - - static __INLINE q31_t arm_bilinear_interp_q31( - arm_bilinear_interp_instance_q31 * S, - q31_t X, - q31_t Y) - { - q31_t out; /* Temporary output */ - q31_t acc = 0; /* output */ - q31_t xfract, yfract; /* X, Y fractional parts */ - q31_t x1, x2, y1, y2; /* Nearest output values */ - int32_t rI, cI; /* Row and column indices */ - q31_t *pYData = S->pData; /* pointer to output table values */ - uint32_t nCols = S->numCols; /* num of rows */ - - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - rI = ((X & 0xFFF00000) >> 20u); - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - cI = ((Y & 0xFFF00000) >> 20u); - - /* Care taken for table outside boundary */ - /* Returns zero output when values are outside table boundary */ - if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) - { - return (0); - } - - /* 20 bits for the fractional part */ - /* shift left xfract by 11 to keep 1.31 format */ - xfract = (X & 0x000FFFFF) << 11u; - - /* Read two nearest output values from the index */ - x1 = pYData[(rI) + nCols * (cI)]; - x2 = pYData[(rI) + nCols * (cI) + 1u]; - - /* 20 bits for the fractional part */ - /* shift left yfract by 11 to keep 1.31 format */ - yfract = (Y & 0x000FFFFF) << 11u; - - /* Read two nearest output values from the index */ - y1 = pYData[(rI) + nCols * (cI + 1)]; - y2 = pYData[(rI) + nCols * (cI + 1) + 1u]; - - /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */ - out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32)); - acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32)); - - /* x2 * (xfract) * (1-yfract) in 3.29(q29) and adding to acc */ - out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32)); - acc += ((q31_t) ((q63_t) out * (xfract) >> 32)); - - /* y1 * (1 - xfract) * (yfract) in 3.29(q29) and adding to acc */ - out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32)); - acc += ((q31_t) ((q63_t) out * (yfract) >> 32)); - - /* y2 * (xfract) * (yfract) in 3.29(q29) and adding to acc */ - out = ((q31_t) ((q63_t) y2 * (xfract) >> 32)); - acc += ((q31_t) ((q63_t) out * (yfract) >> 32)); - - /* Convert acc to 1.31(q31) format */ - return (acc << 2u); - - } - - /** - * @brief Q15 bilinear interpolation. - * @param[in,out] *S points to an instance of the interpolation structure. - * @param[in] X interpolation coordinate in 12.20 format. - * @param[in] Y interpolation coordinate in 12.20 format. - * @return out interpolated value. - */ - - static __INLINE q15_t arm_bilinear_interp_q15( - arm_bilinear_interp_instance_q15 * S, - q31_t X, - q31_t Y) - { - q63_t acc = 0; /* output */ - q31_t out; /* Temporary output */ - q15_t x1, x2, y1, y2; /* Nearest output values */ - q31_t xfract, yfract; /* X, Y fractional parts */ - int32_t rI, cI; /* Row and column indices */ - q15_t *pYData = S->pData; /* pointer to output table values */ - uint32_t nCols = S->numCols; /* num of rows */ - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - rI = ((X & 0xFFF00000) >> 20); - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - cI = ((Y & 0xFFF00000) >> 20); - - /* Care taken for table outside boundary */ - /* Returns zero output when values are outside table boundary */ - if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) - { - return (0); - } - - /* 20 bits for the fractional part */ - /* xfract should be in 12.20 format */ - xfract = (X & 0x000FFFFF); - - /* Read two nearest output values from the index */ - x1 = pYData[(rI) + nCols * (cI)]; - x2 = pYData[(rI) + nCols * (cI) + 1u]; - - - /* 20 bits for the fractional part */ - /* yfract should be in 12.20 format */ - yfract = (Y & 0x000FFFFF); - - /* Read two nearest output values from the index */ - y1 = pYData[(rI) + nCols * (cI + 1)]; - y2 = pYData[(rI) + nCols * (cI + 1) + 1u]; - - /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */ - - /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */ - /* convert 13.35 to 13.31 by right shifting and out is in 1.31 */ - out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u); - acc = ((q63_t) out * (0xFFFFF - yfract)); - - /* x2 * (xfract) * (1-yfract) in 1.51 and adding to acc */ - out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u); - acc += ((q63_t) out * (xfract)); - - /* y1 * (1 - xfract) * (yfract) in 1.51 and adding to acc */ - out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u); - acc += ((q63_t) out * (yfract)); - - /* y2 * (xfract) * (yfract) in 1.51 and adding to acc */ - out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u); - acc += ((q63_t) out * (yfract)); - - /* acc is in 13.51 format and down shift acc by 36 times */ - /* Convert out to 1.15 format */ - return (acc >> 36); - - } - - /** - * @brief Q7 bilinear interpolation. - * @param[in,out] *S points to an instance of the interpolation structure. - * @param[in] X interpolation coordinate in 12.20 format. - * @param[in] Y interpolation coordinate in 12.20 format. - * @return out interpolated value. - */ - - static __INLINE q7_t arm_bilinear_interp_q7( - arm_bilinear_interp_instance_q7 * S, - q31_t X, - q31_t Y) - { - q63_t acc = 0; /* output */ - q31_t out; /* Temporary output */ - q31_t xfract, yfract; /* X, Y fractional parts */ - q7_t x1, x2, y1, y2; /* Nearest output values */ - int32_t rI, cI; /* Row and column indices */ - q7_t *pYData = S->pData; /* pointer to output table values */ - uint32_t nCols = S->numCols; /* num of rows */ - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - rI = ((X & 0xFFF00000) >> 20); - - /* Input is in 12.20 format */ - /* 12 bits for the table index */ - /* Index value calculation */ - cI = ((Y & 0xFFF00000) >> 20); - - /* Care taken for table outside boundary */ - /* Returns zero output when values are outside table boundary */ - if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1)) - { - return (0); - } - - /* 20 bits for the fractional part */ - /* xfract should be in 12.20 format */ - xfract = (X & 0x000FFFFF); - - /* Read two nearest output values from the index */ - x1 = pYData[(rI) + nCols * (cI)]; - x2 = pYData[(rI) + nCols * (cI) + 1u]; - - - /* 20 bits for the fractional part */ - /* yfract should be in 12.20 format */ - yfract = (Y & 0x000FFFFF); - - /* Read two nearest output values from the index */ - y1 = pYData[(rI) + nCols * (cI + 1)]; - y2 = pYData[(rI) + nCols * (cI + 1) + 1u]; - - /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */ - out = ((x1 * (0xFFFFF - xfract))); - acc = (((q63_t) out * (0xFFFFF - yfract))); - - /* x2 * (xfract) * (1-yfract) in 2.22 and adding to acc */ - out = ((x2 * (0xFFFFF - yfract))); - acc += (((q63_t) out * (xfract))); - - /* y1 * (1 - xfract) * (yfract) in 2.22 and adding to acc */ - out = ((y1 * (0xFFFFF - xfract))); - acc += (((q63_t) out * (yfract))); - - /* y2 * (xfract) * (yfract) in 2.22 and adding to acc */ - out = ((y2 * (yfract))); - acc += (((q63_t) out * (xfract))); - - /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */ - return (acc >> 40); - - } - - /** - * @} end of BilinearInterpolate group - */ - - -#if defined ( __CC_ARM ) //Keil -//SMMLAR - #define multAcc_32x32_keep32_R(a, x, y) \ - a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32) - -//SMMLSR - #define multSub_32x32_keep32_R(a, x, y) \ - a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32) - -//SMMULR - #define mult_32x32_keep32_R(a, x, y) \ - a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32) - -//Enter low optimization region - place directly above function definition - #define LOW_OPTIMIZATION_ENTER \ - _Pragma ("push") \ - _Pragma ("O1") - -//Exit low optimization region - place directly after end of function definition - #define LOW_OPTIMIZATION_EXIT \ - _Pragma ("pop") - -//Enter low optimization region - place directly above function definition - #define IAR_ONLY_LOW_OPTIMIZATION_ENTER - -//Exit low optimization region - place directly after end of function definition - #define IAR_ONLY_LOW_OPTIMIZATION_EXIT - -#elif defined(__ICCARM__) //IAR - //SMMLA - #define multAcc_32x32_keep32_R(a, x, y) \ - a += (q31_t) (((q63_t) x * y) >> 32) - - //SMMLS - #define multSub_32x32_keep32_R(a, x, y) \ - a -= (q31_t) (((q63_t) x * y) >> 32) - -//SMMUL - #define mult_32x32_keep32_R(a, x, y) \ - a = (q31_t) (((q63_t) x * y ) >> 32) - -//Enter low optimization region - place directly above function definition - #define LOW_OPTIMIZATION_ENTER \ - _Pragma ("optimize=low") - -//Exit low optimization region - place directly after end of function definition - #define LOW_OPTIMIZATION_EXIT - -//Enter low optimization region - place directly above function definition - #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \ - _Pragma ("optimize=low") - -//Exit low optimization region - place directly after end of function definition - #define IAR_ONLY_LOW_OPTIMIZATION_EXIT - -#elif defined(__GNUC__) - //SMMLA - #define multAcc_32x32_keep32_R(a, x, y) \ - a += (q31_t) (((q63_t) x * y) >> 32) - - //SMMLS - #define multSub_32x32_keep32_R(a, x, y) \ - a -= (q31_t) (((q63_t) x * y) >> 32) - -//SMMUL - #define mult_32x32_keep32_R(a, x, y) \ - a = (q31_t) (((q63_t) x * y ) >> 32) - - #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") )) - - #define LOW_OPTIMIZATION_EXIT - - #define IAR_ONLY_LOW_OPTIMIZATION_ENTER - - #define IAR_ONLY_LOW_OPTIMIZATION_EXIT - -#endif - - - - - -#ifdef __cplusplus -} -#endif - - -#endif /* _ARM_MATH_H */ - - -/** - * - * End of file. - */ diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/modules/mathlib/CMSIS/Include/core_cm3.h deleted file mode 100644 index 8ac6dc078..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cm3.h +++ /dev/null @@ -1,1627 +0,0 @@ -/**************************************************************************//** - * @file core_cm3.h - * @brief CMSIS Cortex-M3 Core Peripheral Access Layer Header File - * @version V3.20 - * @date 25. February 2013 - * - * @note - * - ******************************************************************************/ -/* Copyright (c) 2009 - 2013 ARM LIMITED - - All rights reserved. - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 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. - - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. - ---------------------------------------------------------------------------*/ - - -#if defined ( __ICCARM__ ) - #pragma system_include /* treat file as system include file for MISRA check */ -#endif - -#ifdef __cplusplus - extern "C" { -#endif - -#ifndef __CORE_CM3_H_GENERIC -#define __CORE_CM3_H_GENERIC - -/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions - CMSIS violates the following MISRA-C:2004 rules: - - \li Required Rule 8.5, object/function definition in header file.
- Function definitions in header files are used to allow 'inlining'. - - \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.
- Unions are used for effective representation of core registers. - - \li Advisory Rule 19.7, Function-like macro defined.
- Function-like macros are used to allow more efficient code. - */ - - -/******************************************************************************* - * CMSIS definitions - ******************************************************************************/ -/** \ingroup Cortex_M3 - @{ - */ - -/* CMSIS CM3 definitions */ -#define __CM3_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */ -#define __CM3_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */ -#define __CM3_CMSIS_VERSION ((__CM3_CMSIS_VERSION_MAIN << 16) | \ - __CM3_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */ - -#define __CORTEX_M (0x03) /*!< Cortex-M Core */ - - -#if defined ( __CC_ARM ) - #define __ASM __asm /*!< asm keyword for ARM Compiler */ - #define __INLINE __inline /*!< inline keyword for ARM Compiler */ - #define __STATIC_INLINE static __inline - -#elif defined ( __ICCARM__ ) - #define __ASM __asm /*!< asm keyword for IAR Compiler */ - #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */ - #define __STATIC_INLINE static inline - -#elif defined ( __TMS470__ ) - #define __ASM __asm /*!< asm keyword for TI CCS Compiler */ - #define __STATIC_INLINE static inline - -#elif defined ( __GNUC__ ) - #define __ASM __asm /*!< asm keyword for GNU Compiler */ - #define __INLINE inline /*!< inline keyword for GNU Compiler */ - #define __STATIC_INLINE static inline - -#elif defined ( __TASKING__ ) - #define __ASM __asm /*!< asm keyword for TASKING Compiler */ - #define __INLINE inline /*!< inline keyword for TASKING Compiler */ - #define __STATIC_INLINE static inline - -#endif - -/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all -*/ -#define __FPU_USED 0 - -#if defined ( __CC_ARM ) - #if defined __TARGET_FPU_VFP - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __ICCARM__ ) - #if defined __ARMVFP__ - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __TMS470__ ) - #if defined __TI__VFP_SUPPORT____ - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __GNUC__ ) - #if defined (__VFP_FP__) && !defined(__SOFTFP__) - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __TASKING__ ) - #if defined __FPU_VFP__ - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif -#endif - -#include /* standard types definitions */ -#include "core_cmInstr.h" /* Core Instruction Access */ -#include "core_cmFunc.h" /* Core Function Access */ - -#endif /* __CORE_CM3_H_GENERIC */ - -#ifndef __CMSIS_GENERIC - -#ifndef __CORE_CM3_H_DEPENDANT -#define __CORE_CM3_H_DEPENDANT - -/* check device defines and use defaults */ -#if defined __CHECK_DEVICE_DEFINES - #ifndef __CM3_REV - #define __CM3_REV 0x0200 - #warning "__CM3_REV not defined in device header file; using default!" - #endif - - #ifndef __MPU_PRESENT - #define __MPU_PRESENT 0 - #warning "__MPU_PRESENT not defined in device header file; using default!" - #endif - - #ifndef __NVIC_PRIO_BITS - #define __NVIC_PRIO_BITS 4 - #warning "__NVIC_PRIO_BITS not defined in device header file; using default!" - #endif - - #ifndef __Vendor_SysTickConfig - #define __Vendor_SysTickConfig 0 - #warning "__Vendor_SysTickConfig not defined in device header file; using default!" - #endif -#endif - -/* IO definitions (access restrictions to peripheral registers) */ -/** - \defgroup CMSIS_glob_defs CMSIS Global Defines - - IO Type Qualifiers are used - \li to specify the access to peripheral variables. - \li for automatic generation of peripheral register debug information. -*/ -#ifdef __cplusplus - #define __I volatile /*!< Defines 'read only' permissions */ -#else - #define __I volatile const /*!< Defines 'read only' permissions */ -#endif -#define __O volatile /*!< Defines 'write only' permissions */ -#define __IO volatile /*!< Defines 'read / write' permissions */ - -/*@} end of group Cortex_M3 */ - - - -/******************************************************************************* - * Register Abstraction - Core Register contain: - - Core Register - - Core NVIC Register - - Core SCB Register - - Core SysTick Register - - Core Debug Register - - Core MPU Register - ******************************************************************************/ -/** \defgroup CMSIS_core_register Defines and Type Definitions - \brief Type definitions and defines for Cortex-M processor based devices. -*/ - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_CORE Status and Control Registers - \brief Core Register type definitions. - @{ - */ - -/** \brief Union type to access the Application Program Status Register (APSR). - */ -typedef union -{ - struct - { -#if (__CORTEX_M != 0x04) - uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */ -#else - uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */ - uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ - uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */ -#endif - uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ - uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ - uint32_t C:1; /*!< bit: 29 Carry condition code flag */ - uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ - uint32_t N:1; /*!< bit: 31 Negative condition code flag */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} APSR_Type; - - -/** \brief Union type to access the Interrupt Program Status Register (IPSR). - */ -typedef union -{ - struct - { - uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ - uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} IPSR_Type; - - -/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR). - */ -typedef union -{ - struct - { - uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ -#if (__CORTEX_M != 0x04) - uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */ -#else - uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */ - uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ - uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */ -#endif - uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */ - uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */ - uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ - uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ - uint32_t C:1; /*!< bit: 29 Carry condition code flag */ - uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ - uint32_t N:1; /*!< bit: 31 Negative condition code flag */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} xPSR_Type; - - -/** \brief Union type to access the Control Registers (CONTROL). - */ -typedef union -{ - struct - { - uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */ - uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */ - uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */ - uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} CONTROL_Type; - -/*@} end of group CMSIS_CORE */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC) - \brief Type definitions for the NVIC Registers - @{ - */ - -/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC). - */ -typedef struct -{ - __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ - uint32_t RESERVED0[24]; - __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ - uint32_t RSERVED1[24]; - __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ - uint32_t RESERVED2[24]; - __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ - uint32_t RESERVED3[24]; - __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ - uint32_t RESERVED4[56]; - __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ - uint32_t RESERVED5[644]; - __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ -} NVIC_Type; - -/* Software Triggered Interrupt Register Definitions */ -#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */ -#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */ - -/*@} end of group CMSIS_NVIC */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_SCB System Control Block (SCB) - \brief Type definitions for the System Control Block Registers - @{ - */ - -/** \brief Structure type to access the System Control Block (SCB). - */ -typedef struct -{ - __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */ - __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */ - __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */ - __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */ - __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */ - __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */ - __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */ - __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */ - __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */ - __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */ - __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */ - __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */ - __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */ - __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */ - __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */ - __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */ - __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */ - __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */ - __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */ - uint32_t RESERVED0[5]; - __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */ -} SCB_Type; - -/* SCB CPUID Register Definitions */ -#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */ -#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */ - -#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */ -#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */ - -#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */ -#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */ - -#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */ -#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */ - -#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */ -#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */ - -/* SCB Interrupt Control State Register Definitions */ -#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */ -#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */ - -#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */ -#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */ - -#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */ -#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */ - -#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */ -#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */ - -#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */ -#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */ - -#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */ -#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */ - -#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */ -#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */ - -#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */ -#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */ - -#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */ -#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */ - -#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */ -#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */ - -/* SCB Vector Table Offset Register Definitions */ -#if (__CM3_REV < 0x0201) /* core r2p1 */ -#define SCB_VTOR_TBLBASE_Pos 29 /*!< SCB VTOR: TBLBASE Position */ -#define SCB_VTOR_TBLBASE_Msk (1UL << SCB_VTOR_TBLBASE_Pos) /*!< SCB VTOR: TBLBASE Mask */ - -#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */ -#define SCB_VTOR_TBLOFF_Msk (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ -#else -#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */ -#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ -#endif - -/* SCB Application Interrupt and Reset Control Register Definitions */ -#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */ -#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */ - -#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */ -#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */ - -#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */ -#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */ - -#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */ -#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ - -#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */ -#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */ - -#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */ -#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */ - -#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */ -#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */ - -/* SCB System Control Register Definitions */ -#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */ -#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */ - -#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */ -#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */ - -#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */ -#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */ - -/* SCB Configuration Control Register Definitions */ -#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */ -#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */ - -#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */ -#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */ - -#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */ -#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */ - -#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */ -#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */ - -#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */ -#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */ - -#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */ -#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */ - -/* SCB System Handler Control and State Register Definitions */ -#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */ -#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */ - -#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */ -#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */ - -#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */ -#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */ - -#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */ -#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */ - -#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */ -#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */ - -#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */ -#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */ - -#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */ -#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */ - -#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */ -#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */ - -#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */ -#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */ - -#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */ -#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */ - -#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */ -#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */ - -#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */ -#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */ - -#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */ -#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */ - -#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */ -#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */ - -/* SCB Configurable Fault Status Registers Definitions */ -#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */ -#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */ - -#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */ -#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */ - -#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */ -#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */ - -/* SCB Hard Fault Status Registers Definitions */ -#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */ -#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */ - -#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */ -#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */ - -#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */ -#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */ - -/* SCB Debug Fault Status Register Definitions */ -#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */ -#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */ - -#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */ -#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */ - -#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */ -#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */ - -#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */ -#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */ - -#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */ -#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */ - -/*@} end of group CMSIS_SCB */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB) - \brief Type definitions for the System Control and ID Register not in the SCB - @{ - */ - -/** \brief Structure type to access the System Control and ID Register not in the SCB. - */ -typedef struct -{ - uint32_t RESERVED0[1]; - __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */ -#if ((defined __CM3_REV) && (__CM3_REV >= 0x200)) - __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */ -#else - uint32_t RESERVED1[1]; -#endif -} SCnSCB_Type; - -/* Interrupt Controller Type Register Definitions */ -#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */ -#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */ - -/* Auxiliary Control Register Definitions */ - -#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */ -#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */ - -#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */ -#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */ - -#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */ -#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */ - -/*@} end of group CMSIS_SCnotSCB */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_SysTick System Tick Timer (SysTick) - \brief Type definitions for the System Timer Registers. - @{ - */ - -/** \brief Structure type to access the System Timer (SysTick). - */ -typedef struct -{ - __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */ - __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */ - __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */ - __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */ -} SysTick_Type; - -/* SysTick Control / Status Register Definitions */ -#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */ -#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */ - -#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */ -#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */ - -#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */ -#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */ - -#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */ -#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */ - -/* SysTick Reload Register Definitions */ -#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */ -#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */ - -/* SysTick Current Register Definitions */ -#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */ -#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */ - -/* SysTick Calibration Register Definitions */ -#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */ -#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */ - -#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */ -#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */ - -#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */ -#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */ - -/*@} end of group CMSIS_SysTick */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM) - \brief Type definitions for the Instrumentation Trace Macrocell (ITM) - @{ - */ - -/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM). - */ -typedef struct -{ - __O union - { - __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */ - __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */ - __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */ - } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */ - uint32_t RESERVED0[864]; - __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */ - uint32_t RESERVED1[15]; - __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */ - uint32_t RESERVED2[15]; - __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */ - uint32_t RESERVED3[29]; - __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */ - __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */ - __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */ - uint32_t RESERVED4[43]; - __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */ - __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */ - uint32_t RESERVED5[6]; - __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */ - __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */ - __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */ - __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */ - __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */ - __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */ - __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */ - __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */ - __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */ - __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */ - __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */ - __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */ -} ITM_Type; - -/* ITM Trace Privilege Register Definitions */ -#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */ -#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */ - -/* ITM Trace Control Register Definitions */ -#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */ -#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */ - -#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */ -#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */ - -#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */ -#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */ - -#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */ -#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */ - -#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */ -#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */ - -#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */ -#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */ - -#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */ -#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */ - -#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */ -#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */ - -#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */ -#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */ - -/* ITM Integration Write Register Definitions */ -#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */ -#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */ - -/* ITM Integration Read Register Definitions */ -#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */ -#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */ - -/* ITM Integration Mode Control Register Definitions */ -#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */ -#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */ - -/* ITM Lock Status Register Definitions */ -#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */ -#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */ - -#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */ -#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */ - -#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */ -#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */ - -/*@}*/ /* end of group CMSIS_ITM */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT) - \brief Type definitions for the Data Watchpoint and Trace (DWT) - @{ - */ - -/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT). - */ -typedef struct -{ - __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */ - __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */ - __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */ - __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */ - __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */ - __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */ - __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */ - __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */ - __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */ - __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */ - __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */ - uint32_t RESERVED0[1]; - __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */ - __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */ - __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */ - uint32_t RESERVED1[1]; - __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */ - __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */ - __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */ - uint32_t RESERVED2[1]; - __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */ - __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */ - __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */ -} DWT_Type; - -/* DWT Control Register Definitions */ -#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */ -#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */ - -#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */ -#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */ - -#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */ -#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */ - -#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */ -#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */ - -#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */ -#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */ - -#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */ -#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */ - -#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */ -#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */ - -#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */ -#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */ - -#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */ -#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */ - -#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */ -#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */ - -#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */ -#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */ - -#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */ -#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */ - -#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */ -#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */ - -#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */ -#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */ - -#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */ -#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */ - -#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */ -#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */ - -#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */ -#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */ - -#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */ -#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */ - -/* DWT CPI Count Register Definitions */ -#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */ -#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */ - -/* DWT Exception Overhead Count Register Definitions */ -#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */ -#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */ - -/* DWT Sleep Count Register Definitions */ -#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */ -#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */ - -/* DWT LSU Count Register Definitions */ -#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */ -#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */ - -/* DWT Folded-instruction Count Register Definitions */ -#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */ -#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */ - -/* DWT Comparator Mask Register Definitions */ -#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */ -#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */ - -/* DWT Comparator Function Register Definitions */ -#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */ -#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */ - -#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */ -#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */ - -#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */ -#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */ - -#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */ -#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */ - -#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */ -#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */ - -#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */ -#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */ - -#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */ -#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */ - -#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */ -#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */ - -#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */ -#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */ - -/*@}*/ /* end of group CMSIS_DWT */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_TPI Trace Port Interface (TPI) - \brief Type definitions for the Trace Port Interface (TPI) - @{ - */ - -/** \brief Structure type to access the Trace Port Interface Register (TPI). - */ -typedef struct -{ - __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */ - __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */ - uint32_t RESERVED0[2]; - __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */ - uint32_t RESERVED1[55]; - __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */ - uint32_t RESERVED2[131]; - __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */ - __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */ - __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */ - uint32_t RESERVED3[759]; - __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */ - __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */ - __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */ - uint32_t RESERVED4[1]; - __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */ - __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */ - __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */ - uint32_t RESERVED5[39]; - __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */ - __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */ - uint32_t RESERVED7[8]; - __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */ - __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */ -} TPI_Type; - -/* TPI Asynchronous Clock Prescaler Register Definitions */ -#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */ -#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */ - -/* TPI Selected Pin Protocol Register Definitions */ -#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */ -#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */ - -/* TPI Formatter and Flush Status Register Definitions */ -#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */ -#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */ - -#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */ -#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */ - -#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */ -#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */ - -#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */ -#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */ - -/* TPI Formatter and Flush Control Register Definitions */ -#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */ -#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */ - -#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */ -#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */ - -/* TPI TRIGGER Register Definitions */ -#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */ -#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */ - -/* TPI Integration ETM Data Register Definitions (FIFO0) */ -#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */ -#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */ - -#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */ -#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */ - -#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */ -#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */ - -#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */ -#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */ - -#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */ -#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */ - -#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */ -#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */ - -#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */ -#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */ - -/* TPI ITATBCTR2 Register Definitions */ -#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */ -#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */ - -/* TPI Integration ITM Data Register Definitions (FIFO1) */ -#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */ -#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */ - -#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */ -#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */ - -#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */ -#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */ - -#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */ -#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */ - -#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */ -#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */ - -#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */ -#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */ - -#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */ -#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */ - -/* TPI ITATBCTR0 Register Definitions */ -#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */ -#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */ - -/* TPI Integration Mode Control Register Definitions */ -#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */ -#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */ - -/* TPI DEVID Register Definitions */ -#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */ -#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */ - -#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */ -#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */ - -#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */ -#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */ - -#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */ -#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */ - -#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */ -#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */ - -#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */ -#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */ - -/* TPI DEVTYPE Register Definitions */ -#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */ -#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */ - -#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */ -#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */ - -/*@}*/ /* end of group CMSIS_TPI */ - - -#if (__MPU_PRESENT == 1) -/** \ingroup CMSIS_core_register - \defgroup CMSIS_MPU Memory Protection Unit (MPU) - \brief Type definitions for the Memory Protection Unit (MPU) - @{ - */ - -/** \brief Structure type to access the Memory Protection Unit (MPU). - */ -typedef struct -{ - __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */ - __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */ - __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */ - __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */ - __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */ - __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */ - __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */ - __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */ - __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */ - __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */ - __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */ -} MPU_Type; - -/* MPU Type Register */ -#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */ -#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */ - -#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */ -#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */ - -#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */ -#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */ - -/* MPU Control Register */ -#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */ -#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */ - -#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */ -#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */ - -#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */ -#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */ - -/* MPU Region Number Register */ -#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */ -#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */ - -/* MPU Region Base Address Register */ -#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */ -#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */ - -#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */ -#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */ - -#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */ -#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */ - -/* MPU Region Attribute and Size Register */ -#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */ -#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */ - -#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */ -#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */ - -#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */ -#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */ - -#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */ -#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */ - -#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */ -#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */ - -#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */ -#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */ - -#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */ -#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */ - -#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */ -#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */ - -#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */ -#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */ - -#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */ -#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */ - -/*@} end of group CMSIS_MPU */ -#endif - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug) - \brief Type definitions for the Core Debug Registers - @{ - */ - -/** \brief Structure type to access the Core Debug Register (CoreDebug). - */ -typedef struct -{ - __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */ - __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */ - __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */ - __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */ -} CoreDebug_Type; - -/* Debug Halting Control and Status Register */ -#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */ -#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */ - -#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */ -#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */ - -#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */ -#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */ - -#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */ -#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */ - -#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */ -#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */ - -#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */ -#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */ - -#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */ -#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */ - -#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */ -#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */ - -#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */ -#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */ - -#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */ -#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */ - -#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */ -#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */ - -#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */ -#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */ - -/* Debug Core Register Selector Register */ -#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */ -#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */ - -#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */ -#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */ - -/* Debug Exception and Monitor Control Register */ -#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */ -#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */ - -#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */ -#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */ - -#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */ -#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */ - -#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */ -#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */ - -#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */ -#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */ - -#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */ -#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */ - -#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */ -#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */ - -#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */ -#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */ - -#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */ -#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */ - -#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */ -#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */ - -#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */ -#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */ - -#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */ -#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */ - -#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */ -#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */ - -/*@} end of group CMSIS_CoreDebug */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_core_base Core Definitions - \brief Definitions for base addresses, unions, and structures. - @{ - */ - -/* Memory mapping of Cortex-M3 Hardware */ -#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */ -#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */ -#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */ -#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */ -#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */ -#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */ -#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */ -#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */ - -#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */ -#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */ -#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */ -#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */ -#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */ -#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */ -#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */ -#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */ - -#if (__MPU_PRESENT == 1) - #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */ - #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */ -#endif - -/*@} */ - - - -/******************************************************************************* - * Hardware Abstraction Layer - Core Function Interface contains: - - Core NVIC Functions - - Core SysTick Functions - - Core Debug Functions - - Core Register Access Functions - ******************************************************************************/ -/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference -*/ - - - -/* ########################## NVIC functions #################################### */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_NVICFunctions NVIC Functions - \brief Functions that manage interrupts and exceptions via the NVIC. - @{ - */ - -/** \brief Set Priority Grouping - - The function sets the priority grouping field using the required unlock sequence. - The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field. - Only values from 0..7 are used. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set. - - \param [in] PriorityGroup Priority grouping field. - */ -__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) -{ - uint32_t reg_value; - uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */ - - reg_value = SCB->AIRCR; /* read old register configuration */ - reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */ - reg_value = (reg_value | - ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) | - (PriorityGroupTmp << 8)); /* Insert write key and priorty group */ - SCB->AIRCR = reg_value; -} - - -/** \brief Get Priority Grouping - - The function reads the priority grouping field from the NVIC Interrupt Controller. - - \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field). - */ -__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void) -{ - return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */ -} - - -/** \brief Enable External Interrupt - - The function enables a device-specific interrupt in the NVIC interrupt controller. - - \param [in] IRQn External interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn) -{ - NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */ -} - - -/** \brief Disable External Interrupt - - The function disables a device-specific interrupt in the NVIC interrupt controller. - - \param [in] IRQn External interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn) -{ - NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */ -} - - -/** \brief Get Pending Interrupt - - The function reads the pending register in the NVIC and returns the pending bit - for the specified interrupt. - - \param [in] IRQn Interrupt number. - - \return 0 Interrupt status is not pending. - \return 1 Interrupt status is pending. - */ -__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn) -{ - return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */ -} - - -/** \brief Set Pending Interrupt - - The function sets the pending bit of an external interrupt. - - \param [in] IRQn Interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn) -{ - NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */ -} - - -/** \brief Clear Pending Interrupt - - The function clears the pending bit of an external interrupt. - - \param [in] IRQn External interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn) -{ - NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */ -} - - -/** \brief Get Active Interrupt - - The function reads the active register in NVIC and returns the active bit. - - \param [in] IRQn Interrupt number. - - \return 0 Interrupt status is not active. - \return 1 Interrupt status is active. - */ -__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn) -{ - return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */ -} - - -/** \brief Set Interrupt Priority - - The function sets the priority of an interrupt. - - \note The priority cannot be set for every core interrupt. - - \param [in] IRQn Interrupt number. - \param [in] priority Priority to set. - */ -__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority) -{ - if(IRQn < 0) { - SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */ - else { - NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */ -} - - -/** \brief Get Interrupt Priority - - The function reads the priority of an interrupt. The interrupt - number can be positive to specify an external (device specific) - interrupt, or negative to specify an internal (core) interrupt. - - - \param [in] IRQn Interrupt number. - \return Interrupt Priority. Value is aligned automatically to the implemented - priority bits of the microcontroller. - */ -__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn) -{ - - if(IRQn < 0) { - return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */ - else { - return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */ -} - - -/** \brief Encode Priority - - The function encodes the priority for an interrupt with the given priority group, - preemptive priority value, and subpriority value. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set. - - \param [in] PriorityGroup Used priority group. - \param [in] PreemptPriority Preemptive priority value (starting from 0). - \param [in] SubPriority Subpriority value (starting from 0). - \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority(). - */ -__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority) -{ - uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ - uint32_t PreemptPriorityBits; - uint32_t SubPriorityBits; - - PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; - SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; - - return ( - ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) | - ((SubPriority & ((1 << (SubPriorityBits )) - 1))) - ); -} - - -/** \brief Decode Priority - - The function decodes an interrupt priority value with a given priority group to - preemptive priority value and subpriority value. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set. - - \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority(). - \param [in] PriorityGroup Used priority group. - \param [out] pPreemptPriority Preemptive priority value (starting from 0). - \param [out] pSubPriority Subpriority value (starting from 0). - */ -__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority) -{ - uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ - uint32_t PreemptPriorityBits; - uint32_t SubPriorityBits; - - PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; - SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; - - *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1); - *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1); -} - - -/** \brief System Reset - - The function initiates a system reset request to reset the MCU. - */ -__STATIC_INLINE void NVIC_SystemReset(void) -{ - __DSB(); /* Ensure all outstanding memory accesses included - buffered write are completed before reset */ - SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) | - (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | - SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */ - __DSB(); /* Ensure completion of memory access */ - while(1); /* wait until reset */ -} - -/*@} end of CMSIS_Core_NVICFunctions */ - - - -/* ################################## SysTick function ############################################ */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_SysTickFunctions SysTick Functions - \brief Functions that configure the System. - @{ - */ - -#if (__Vendor_SysTickConfig == 0) - -/** \brief System Tick Configuration - - The function initializes the System Timer and its interrupt, and starts the System Tick Timer. - Counter is in free running mode to generate periodic interrupts. - - \param [in] ticks Number of ticks between two interrupts. - - \return 0 Function succeeded. - \return 1 Function failed. - - \note When the variable __Vendor_SysTickConfig is set to 1, then the - function SysTick_Config is not included. In this case, the file device.h - must contain a vendor-specific implementation of this function. - - */ -__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks) -{ - if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */ - - SysTick->LOAD = ticks - 1; /* set reload register */ - NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */ - SysTick->VAL = 0; /* Load the SysTick Counter Value */ - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | - SysTick_CTRL_TICKINT_Msk | - SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */ - return (0); /* Function successful */ -} - -#endif - -/*@} end of CMSIS_Core_SysTickFunctions */ - - - -/* ##################################### Debug In/Output function ########################################### */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_core_DebugFunctions ITM Functions - \brief Functions that access the ITM debug interface. - @{ - */ - -extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */ -#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */ - - -/** \brief ITM Send Character - - The function transmits a character via the ITM channel 0, and - \li Just returns when no debugger is connected that has booked the output. - \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted. - - \param [in] ch Character to transmit. - - \returns Character to transmit. - */ -__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch) -{ - if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */ - (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */ - { - while (ITM->PORT[0].u32 == 0); - ITM->PORT[0].u8 = (uint8_t) ch; - } - return (ch); -} - - -/** \brief ITM Receive Character - - The function inputs a character via the external variable \ref ITM_RxBuffer. - - \return Received character. - \return -1 No character pending. - */ -__STATIC_INLINE int32_t ITM_ReceiveChar (void) { - int32_t ch = -1; /* no character available */ - - if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) { - ch = ITM_RxBuffer; - ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */ - } - - return (ch); -} - - -/** \brief ITM Check Character - - The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer. - - \return 0 No character available. - \return 1 Character available. - */ -__STATIC_INLINE int32_t ITM_CheckChar (void) { - - if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) { - return (0); /* no character available */ - } else { - return (1); /* character available */ - } -} - -/*@} end of CMSIS_core_DebugFunctions */ - -#endif /* __CORE_CM3_H_DEPENDANT */ - -#endif /* __CMSIS_GENERIC */ - -#ifdef __cplusplus -} -#endif diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/modules/mathlib/CMSIS/Include/core_cm4.h deleted file mode 100644 index 93efd3a7a..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cm4.h +++ /dev/null @@ -1,1772 +0,0 @@ -/**************************************************************************//** - * @file core_cm4.h - * @brief CMSIS Cortex-M4 Core Peripheral Access Layer Header File - * @version V3.20 - * @date 25. February 2013 - * - * @note - * - ******************************************************************************/ -/* Copyright (c) 2009 - 2013 ARM LIMITED - - All rights reserved. - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 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. - - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. - ---------------------------------------------------------------------------*/ - - -#if defined ( __ICCARM__ ) - #pragma system_include /* treat file as system include file for MISRA check */ -#endif - -#ifdef __cplusplus - extern "C" { -#endif - -#ifndef __CORE_CM4_H_GENERIC -#define __CORE_CM4_H_GENERIC - -/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions - CMSIS violates the following MISRA-C:2004 rules: - - \li Required Rule 8.5, object/function definition in header file.
- Function definitions in header files are used to allow 'inlining'. - - \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.
- Unions are used for effective representation of core registers. - - \li Advisory Rule 19.7, Function-like macro defined.
- Function-like macros are used to allow more efficient code. - */ - - -/******************************************************************************* - * CMSIS definitions - ******************************************************************************/ -/** \ingroup Cortex_M4 - @{ - */ - -/* CMSIS CM4 definitions */ -#define __CM4_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */ -#define __CM4_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */ -#define __CM4_CMSIS_VERSION ((__CM4_CMSIS_VERSION_MAIN << 16) | \ - __CM4_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */ - -#define __CORTEX_M (0x04) /*!< Cortex-M Core */ - - -#if defined ( __CC_ARM ) - #define __ASM __asm /*!< asm keyword for ARM Compiler */ - #define __INLINE __inline /*!< inline keyword for ARM Compiler */ - #define __STATIC_INLINE static __inline - -#elif defined ( __ICCARM__ ) - #define __ASM __asm /*!< asm keyword for IAR Compiler */ - #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */ - #define __STATIC_INLINE static inline - -#elif defined ( __TMS470__ ) - #define __ASM __asm /*!< asm keyword for TI CCS Compiler */ - #define __STATIC_INLINE static inline - -#elif defined ( __GNUC__ ) - #define __ASM __asm /*!< asm keyword for GNU Compiler */ - #define __INLINE inline /*!< inline keyword for GNU Compiler */ - #define __STATIC_INLINE static inline - -#elif defined ( __TASKING__ ) - #define __ASM __asm /*!< asm keyword for TASKING Compiler */ - #define __INLINE inline /*!< inline keyword for TASKING Compiler */ - #define __STATIC_INLINE static inline - -#endif - -/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions. -*/ -#if defined ( __CC_ARM ) - #if defined __TARGET_FPU_VFP - #if (__FPU_PRESENT == 1) - #define __FPU_USED 1 - #else - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #define __FPU_USED 0 - #endif - #else - #define __FPU_USED 0 - #endif - -#elif defined ( __ICCARM__ ) - #if defined __ARMVFP__ - #if (__FPU_PRESENT == 1) - #define __FPU_USED 1 - #else - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #define __FPU_USED 0 - #endif - #else - #define __FPU_USED 0 - #endif - -#elif defined ( __TMS470__ ) - #if defined __TI_VFP_SUPPORT__ - #if (__FPU_PRESENT == 1) - #define __FPU_USED 1 - #else - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #define __FPU_USED 0 - #endif - #else - #define __FPU_USED 0 - #endif - -#elif defined ( __GNUC__ ) - #if defined (__VFP_FP__) && !defined(__SOFTFP__) - #if (__FPU_PRESENT == 1) - #define __FPU_USED 1 - #else - #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #define __FPU_USED 0 - #endif - #else - #define __FPU_USED 0 - #endif - -#elif defined ( __TASKING__ ) - #if defined __FPU_VFP__ - #if (__FPU_PRESENT == 1) - #define __FPU_USED 1 - #else - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #define __FPU_USED 0 - #endif - #else - #define __FPU_USED 0 - #endif -#endif - -#include /* standard types definitions */ -#include "core_cmInstr.h" /* Core Instruction Access */ -#include "core_cmFunc.h" /* Core Function Access */ -#include "core_cm4_simd.h" /* Compiler specific SIMD Intrinsics */ - -#endif /* __CORE_CM4_H_GENERIC */ - -#ifndef __CMSIS_GENERIC - -#ifndef __CORE_CM4_H_DEPENDANT -#define __CORE_CM4_H_DEPENDANT - -/* check device defines and use defaults */ -#if defined __CHECK_DEVICE_DEFINES - #ifndef __CM4_REV - #define __CM4_REV 0x0000 - #warning "__CM4_REV not defined in device header file; using default!" - #endif - - #ifndef __FPU_PRESENT - #define __FPU_PRESENT 0 - #warning "__FPU_PRESENT not defined in device header file; using default!" - #endif - - #ifndef __MPU_PRESENT - #define __MPU_PRESENT 0 - #warning "__MPU_PRESENT not defined in device header file; using default!" - #endif - - #ifndef __NVIC_PRIO_BITS - #define __NVIC_PRIO_BITS 4 - #warning "__NVIC_PRIO_BITS not defined in device header file; using default!" - #endif - - #ifndef __Vendor_SysTickConfig - #define __Vendor_SysTickConfig 0 - #warning "__Vendor_SysTickConfig not defined in device header file; using default!" - #endif -#endif - -/* IO definitions (access restrictions to peripheral registers) */ -/** - \defgroup CMSIS_glob_defs CMSIS Global Defines - - IO Type Qualifiers are used - \li to specify the access to peripheral variables. - \li for automatic generation of peripheral register debug information. -*/ -#ifdef __cplusplus - #define __I volatile /*!< Defines 'read only' permissions */ -#else - #define __I volatile const /*!< Defines 'read only' permissions */ -#endif -#define __O volatile /*!< Defines 'write only' permissions */ -#define __IO volatile /*!< Defines 'read / write' permissions */ - -/*@} end of group Cortex_M4 */ - - - -/******************************************************************************* - * Register Abstraction - Core Register contain: - - Core Register - - Core NVIC Register - - Core SCB Register - - Core SysTick Register - - Core Debug Register - - Core MPU Register - - Core FPU Register - ******************************************************************************/ -/** \defgroup CMSIS_core_register Defines and Type Definitions - \brief Type definitions and defines for Cortex-M processor based devices. -*/ - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_CORE Status and Control Registers - \brief Core Register type definitions. - @{ - */ - -/** \brief Union type to access the Application Program Status Register (APSR). - */ -typedef union -{ - struct - { -#if (__CORTEX_M != 0x04) - uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */ -#else - uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */ - uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ - uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */ -#endif - uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ - uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ - uint32_t C:1; /*!< bit: 29 Carry condition code flag */ - uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ - uint32_t N:1; /*!< bit: 31 Negative condition code flag */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} APSR_Type; - - -/** \brief Union type to access the Interrupt Program Status Register (IPSR). - */ -typedef union -{ - struct - { - uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ - uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} IPSR_Type; - - -/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR). - */ -typedef union -{ - struct - { - uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ -#if (__CORTEX_M != 0x04) - uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */ -#else - uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */ - uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */ - uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */ -#endif - uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */ - uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */ - uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ - uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ - uint32_t C:1; /*!< bit: 29 Carry condition code flag */ - uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ - uint32_t N:1; /*!< bit: 31 Negative condition code flag */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} xPSR_Type; - - -/** \brief Union type to access the Control Registers (CONTROL). - */ -typedef union -{ - struct - { - uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */ - uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */ - uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */ - uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} CONTROL_Type; - -/*@} end of group CMSIS_CORE */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC) - \brief Type definitions for the NVIC Registers - @{ - */ - -/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC). - */ -typedef struct -{ - __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ - uint32_t RESERVED0[24]; - __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ - uint32_t RSERVED1[24]; - __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ - uint32_t RESERVED2[24]; - __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ - uint32_t RESERVED3[24]; - __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ - uint32_t RESERVED4[56]; - __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ - uint32_t RESERVED5[644]; - __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ -} NVIC_Type; - -/* Software Triggered Interrupt Register Definitions */ -#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */ -#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */ - -/*@} end of group CMSIS_NVIC */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_SCB System Control Block (SCB) - \brief Type definitions for the System Control Block Registers - @{ - */ - -/** \brief Structure type to access the System Control Block (SCB). - */ -typedef struct -{ - __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */ - __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */ - __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */ - __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */ - __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */ - __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */ - __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */ - __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */ - __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */ - __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */ - __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */ - __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */ - __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */ - __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */ - __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */ - __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */ - __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */ - __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */ - __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */ - uint32_t RESERVED0[5]; - __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */ -} SCB_Type; - -/* SCB CPUID Register Definitions */ -#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */ -#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */ - -#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */ -#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */ - -#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */ -#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */ - -#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */ -#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */ - -#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */ -#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */ - -/* SCB Interrupt Control State Register Definitions */ -#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */ -#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */ - -#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */ -#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */ - -#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */ -#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */ - -#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */ -#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */ - -#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */ -#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */ - -#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */ -#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */ - -#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */ -#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */ - -#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */ -#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */ - -#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */ -#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */ - -#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */ -#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */ - -/* SCB Vector Table Offset Register Definitions */ -#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */ -#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ - -/* SCB Application Interrupt and Reset Control Register Definitions */ -#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */ -#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */ - -#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */ -#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */ - -#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */ -#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */ - -#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */ -#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ - -#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */ -#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */ - -#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */ -#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */ - -#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */ -#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */ - -/* SCB System Control Register Definitions */ -#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */ -#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */ - -#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */ -#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */ - -#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */ -#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */ - -/* SCB Configuration Control Register Definitions */ -#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */ -#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */ - -#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */ -#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */ - -#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */ -#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */ - -#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */ -#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */ - -#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */ -#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */ - -#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */ -#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */ - -/* SCB System Handler Control and State Register Definitions */ -#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */ -#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */ - -#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */ -#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */ - -#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */ -#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */ - -#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */ -#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */ - -#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */ -#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */ - -#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */ -#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */ - -#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */ -#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */ - -#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */ -#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */ - -#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */ -#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */ - -#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */ -#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */ - -#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */ -#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */ - -#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */ -#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */ - -#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */ -#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */ - -#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */ -#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */ - -/* SCB Configurable Fault Status Registers Definitions */ -#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */ -#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */ - -#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */ -#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */ - -#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */ -#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */ - -/* SCB Hard Fault Status Registers Definitions */ -#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */ -#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */ - -#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */ -#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */ - -#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */ -#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */ - -/* SCB Debug Fault Status Register Definitions */ -#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */ -#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */ - -#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */ -#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */ - -#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */ -#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */ - -#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */ -#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */ - -#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */ -#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */ - -/*@} end of group CMSIS_SCB */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB) - \brief Type definitions for the System Control and ID Register not in the SCB - @{ - */ - -/** \brief Structure type to access the System Control and ID Register not in the SCB. - */ -typedef struct -{ - uint32_t RESERVED0[1]; - __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */ - __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */ -} SCnSCB_Type; - -/* Interrupt Controller Type Register Definitions */ -#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */ -#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */ - -/* Auxiliary Control Register Definitions */ -#define SCnSCB_ACTLR_DISOOFP_Pos 9 /*!< ACTLR: DISOOFP Position */ -#define SCnSCB_ACTLR_DISOOFP_Msk (1UL << SCnSCB_ACTLR_DISOOFP_Pos) /*!< ACTLR: DISOOFP Mask */ - -#define SCnSCB_ACTLR_DISFPCA_Pos 8 /*!< ACTLR: DISFPCA Position */ -#define SCnSCB_ACTLR_DISFPCA_Msk (1UL << SCnSCB_ACTLR_DISFPCA_Pos) /*!< ACTLR: DISFPCA Mask */ - -#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */ -#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */ - -#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */ -#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */ - -#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */ -#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */ - -/*@} end of group CMSIS_SCnotSCB */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_SysTick System Tick Timer (SysTick) - \brief Type definitions for the System Timer Registers. - @{ - */ - -/** \brief Structure type to access the System Timer (SysTick). - */ -typedef struct -{ - __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */ - __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */ - __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */ - __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */ -} SysTick_Type; - -/* SysTick Control / Status Register Definitions */ -#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */ -#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */ - -#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */ -#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */ - -#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */ -#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */ - -#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */ -#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */ - -/* SysTick Reload Register Definitions */ -#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */ -#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */ - -/* SysTick Current Register Definitions */ -#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */ -#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */ - -/* SysTick Calibration Register Definitions */ -#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */ -#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */ - -#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */ -#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */ - -#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */ -#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */ - -/*@} end of group CMSIS_SysTick */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM) - \brief Type definitions for the Instrumentation Trace Macrocell (ITM) - @{ - */ - -/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM). - */ -typedef struct -{ - __O union - { - __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */ - __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */ - __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */ - } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */ - uint32_t RESERVED0[864]; - __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */ - uint32_t RESERVED1[15]; - __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */ - uint32_t RESERVED2[15]; - __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */ - uint32_t RESERVED3[29]; - __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */ - __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */ - __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */ - uint32_t RESERVED4[43]; - __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */ - __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */ - uint32_t RESERVED5[6]; - __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */ - __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */ - __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */ - __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */ - __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */ - __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */ - __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */ - __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */ - __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */ - __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */ - __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */ - __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */ -} ITM_Type; - -/* ITM Trace Privilege Register Definitions */ -#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */ -#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */ - -/* ITM Trace Control Register Definitions */ -#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */ -#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */ - -#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */ -#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */ - -#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */ -#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */ - -#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */ -#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */ - -#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */ -#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */ - -#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */ -#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */ - -#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */ -#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */ - -#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */ -#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */ - -#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */ -#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */ - -/* ITM Integration Write Register Definitions */ -#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */ -#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */ - -/* ITM Integration Read Register Definitions */ -#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */ -#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */ - -/* ITM Integration Mode Control Register Definitions */ -#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */ -#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */ - -/* ITM Lock Status Register Definitions */ -#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */ -#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */ - -#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */ -#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */ - -#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */ -#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */ - -/*@}*/ /* end of group CMSIS_ITM */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT) - \brief Type definitions for the Data Watchpoint and Trace (DWT) - @{ - */ - -/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT). - */ -typedef struct -{ - __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */ - __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */ - __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */ - __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */ - __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */ - __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */ - __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */ - __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */ - __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */ - __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */ - __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */ - uint32_t RESERVED0[1]; - __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */ - __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */ - __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */ - uint32_t RESERVED1[1]; - __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */ - __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */ - __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */ - uint32_t RESERVED2[1]; - __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */ - __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */ - __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */ -} DWT_Type; - -/* DWT Control Register Definitions */ -#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */ -#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */ - -#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */ -#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */ - -#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */ -#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */ - -#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */ -#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */ - -#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */ -#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */ - -#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */ -#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */ - -#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */ -#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */ - -#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */ -#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */ - -#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */ -#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */ - -#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */ -#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */ - -#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */ -#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */ - -#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */ -#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */ - -#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */ -#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */ - -#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */ -#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */ - -#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */ -#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */ - -#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */ -#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */ - -#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */ -#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */ - -#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */ -#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */ - -/* DWT CPI Count Register Definitions */ -#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */ -#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */ - -/* DWT Exception Overhead Count Register Definitions */ -#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */ -#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */ - -/* DWT Sleep Count Register Definitions */ -#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */ -#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */ - -/* DWT LSU Count Register Definitions */ -#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */ -#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */ - -/* DWT Folded-instruction Count Register Definitions */ -#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */ -#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */ - -/* DWT Comparator Mask Register Definitions */ -#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */ -#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */ - -/* DWT Comparator Function Register Definitions */ -#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */ -#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */ - -#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */ -#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */ - -#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */ -#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */ - -#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */ -#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */ - -#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */ -#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */ - -#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */ -#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */ - -#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */ -#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */ - -#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */ -#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */ - -#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */ -#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */ - -/*@}*/ /* end of group CMSIS_DWT */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_TPI Trace Port Interface (TPI) - \brief Type definitions for the Trace Port Interface (TPI) - @{ - */ - -/** \brief Structure type to access the Trace Port Interface Register (TPI). - */ -typedef struct -{ - __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */ - __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */ - uint32_t RESERVED0[2]; - __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */ - uint32_t RESERVED1[55]; - __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */ - uint32_t RESERVED2[131]; - __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */ - __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */ - __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */ - uint32_t RESERVED3[759]; - __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */ - __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */ - __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */ - uint32_t RESERVED4[1]; - __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */ - __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */ - __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */ - uint32_t RESERVED5[39]; - __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */ - __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */ - uint32_t RESERVED7[8]; - __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */ - __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */ -} TPI_Type; - -/* TPI Asynchronous Clock Prescaler Register Definitions */ -#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */ -#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */ - -/* TPI Selected Pin Protocol Register Definitions */ -#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */ -#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */ - -/* TPI Formatter and Flush Status Register Definitions */ -#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */ -#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */ - -#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */ -#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */ - -#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */ -#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */ - -#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */ -#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */ - -/* TPI Formatter and Flush Control Register Definitions */ -#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */ -#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */ - -#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */ -#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */ - -/* TPI TRIGGER Register Definitions */ -#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */ -#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */ - -/* TPI Integration ETM Data Register Definitions (FIFO0) */ -#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */ -#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */ - -#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */ -#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */ - -#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */ -#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */ - -#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */ -#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */ - -#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */ -#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */ - -#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */ -#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */ - -#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */ -#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */ - -/* TPI ITATBCTR2 Register Definitions */ -#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */ -#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */ - -/* TPI Integration ITM Data Register Definitions (FIFO1) */ -#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */ -#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */ - -#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */ -#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */ - -#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */ -#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */ - -#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */ -#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */ - -#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */ -#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */ - -#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */ -#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */ - -#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */ -#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */ - -/* TPI ITATBCTR0 Register Definitions */ -#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */ -#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */ - -/* TPI Integration Mode Control Register Definitions */ -#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */ -#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */ - -/* TPI DEVID Register Definitions */ -#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */ -#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */ - -#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */ -#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */ - -#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */ -#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */ - -#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */ -#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */ - -#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */ -#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */ - -#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */ -#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */ - -/* TPI DEVTYPE Register Definitions */ -#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */ -#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */ - -#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */ -#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */ - -/*@}*/ /* end of group CMSIS_TPI */ - - -#if (__MPU_PRESENT == 1) -/** \ingroup CMSIS_core_register - \defgroup CMSIS_MPU Memory Protection Unit (MPU) - \brief Type definitions for the Memory Protection Unit (MPU) - @{ - */ - -/** \brief Structure type to access the Memory Protection Unit (MPU). - */ -typedef struct -{ - __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */ - __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */ - __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */ - __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */ - __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */ - __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */ - __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */ - __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */ - __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */ - __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */ - __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */ -} MPU_Type; - -/* MPU Type Register */ -#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */ -#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */ - -#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */ -#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */ - -#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */ -#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */ - -/* MPU Control Register */ -#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */ -#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */ - -#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */ -#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */ - -#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */ -#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */ - -/* MPU Region Number Register */ -#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */ -#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */ - -/* MPU Region Base Address Register */ -#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */ -#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */ - -#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */ -#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */ - -#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */ -#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */ - -/* MPU Region Attribute and Size Register */ -#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */ -#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */ - -#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */ -#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */ - -#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */ -#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */ - -#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */ -#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */ - -#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */ -#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */ - -#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */ -#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */ - -#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */ -#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */ - -#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */ -#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */ - -#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */ -#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */ - -#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */ -#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */ - -/*@} end of group CMSIS_MPU */ -#endif - - -#if (__FPU_PRESENT == 1) -/** \ingroup CMSIS_core_register - \defgroup CMSIS_FPU Floating Point Unit (FPU) - \brief Type definitions for the Floating Point Unit (FPU) - @{ - */ - -/** \brief Structure type to access the Floating Point Unit (FPU). - */ -typedef struct -{ - uint32_t RESERVED0[1]; - __IO uint32_t FPCCR; /*!< Offset: 0x004 (R/W) Floating-Point Context Control Register */ - __IO uint32_t FPCAR; /*!< Offset: 0x008 (R/W) Floating-Point Context Address Register */ - __IO uint32_t FPDSCR; /*!< Offset: 0x00C (R/W) Floating-Point Default Status Control Register */ - __I uint32_t MVFR0; /*!< Offset: 0x010 (R/ ) Media and FP Feature Register 0 */ - __I uint32_t MVFR1; /*!< Offset: 0x014 (R/ ) Media and FP Feature Register 1 */ -} FPU_Type; - -/* Floating-Point Context Control Register */ -#define FPU_FPCCR_ASPEN_Pos 31 /*!< FPCCR: ASPEN bit Position */ -#define FPU_FPCCR_ASPEN_Msk (1UL << FPU_FPCCR_ASPEN_Pos) /*!< FPCCR: ASPEN bit Mask */ - -#define FPU_FPCCR_LSPEN_Pos 30 /*!< FPCCR: LSPEN Position */ -#define FPU_FPCCR_LSPEN_Msk (1UL << FPU_FPCCR_LSPEN_Pos) /*!< FPCCR: LSPEN bit Mask */ - -#define FPU_FPCCR_MONRDY_Pos 8 /*!< FPCCR: MONRDY Position */ -#define FPU_FPCCR_MONRDY_Msk (1UL << FPU_FPCCR_MONRDY_Pos) /*!< FPCCR: MONRDY bit Mask */ - -#define FPU_FPCCR_BFRDY_Pos 6 /*!< FPCCR: BFRDY Position */ -#define FPU_FPCCR_BFRDY_Msk (1UL << FPU_FPCCR_BFRDY_Pos) /*!< FPCCR: BFRDY bit Mask */ - -#define FPU_FPCCR_MMRDY_Pos 5 /*!< FPCCR: MMRDY Position */ -#define FPU_FPCCR_MMRDY_Msk (1UL << FPU_FPCCR_MMRDY_Pos) /*!< FPCCR: MMRDY bit Mask */ - -#define FPU_FPCCR_HFRDY_Pos 4 /*!< FPCCR: HFRDY Position */ -#define FPU_FPCCR_HFRDY_Msk (1UL << FPU_FPCCR_HFRDY_Pos) /*!< FPCCR: HFRDY bit Mask */ - -#define FPU_FPCCR_THREAD_Pos 3 /*!< FPCCR: processor mode bit Position */ -#define FPU_FPCCR_THREAD_Msk (1UL << FPU_FPCCR_THREAD_Pos) /*!< FPCCR: processor mode active bit Mask */ - -#define FPU_FPCCR_USER_Pos 1 /*!< FPCCR: privilege level bit Position */ -#define FPU_FPCCR_USER_Msk (1UL << FPU_FPCCR_USER_Pos) /*!< FPCCR: privilege level bit Mask */ - -#define FPU_FPCCR_LSPACT_Pos 0 /*!< FPCCR: Lazy state preservation active bit Position */ -#define FPU_FPCCR_LSPACT_Msk (1UL << FPU_FPCCR_LSPACT_Pos) /*!< FPCCR: Lazy state preservation active bit Mask */ - -/* Floating-Point Context Address Register */ -#define FPU_FPCAR_ADDRESS_Pos 3 /*!< FPCAR: ADDRESS bit Position */ -#define FPU_FPCAR_ADDRESS_Msk (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos) /*!< FPCAR: ADDRESS bit Mask */ - -/* Floating-Point Default Status Control Register */ -#define FPU_FPDSCR_AHP_Pos 26 /*!< FPDSCR: AHP bit Position */ -#define FPU_FPDSCR_AHP_Msk (1UL << FPU_FPDSCR_AHP_Pos) /*!< FPDSCR: AHP bit Mask */ - -#define FPU_FPDSCR_DN_Pos 25 /*!< FPDSCR: DN bit Position */ -#define FPU_FPDSCR_DN_Msk (1UL << FPU_FPDSCR_DN_Pos) /*!< FPDSCR: DN bit Mask */ - -#define FPU_FPDSCR_FZ_Pos 24 /*!< FPDSCR: FZ bit Position */ -#define FPU_FPDSCR_FZ_Msk (1UL << FPU_FPDSCR_FZ_Pos) /*!< FPDSCR: FZ bit Mask */ - -#define FPU_FPDSCR_RMode_Pos 22 /*!< FPDSCR: RMode bit Position */ -#define FPU_FPDSCR_RMode_Msk (3UL << FPU_FPDSCR_RMode_Pos) /*!< FPDSCR: RMode bit Mask */ - -/* Media and FP Feature Register 0 */ -#define FPU_MVFR0_FP_rounding_modes_Pos 28 /*!< MVFR0: FP rounding modes bits Position */ -#define FPU_MVFR0_FP_rounding_modes_Msk (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos) /*!< MVFR0: FP rounding modes bits Mask */ - -#define FPU_MVFR0_Short_vectors_Pos 24 /*!< MVFR0: Short vectors bits Position */ -#define FPU_MVFR0_Short_vectors_Msk (0xFUL << FPU_MVFR0_Short_vectors_Pos) /*!< MVFR0: Short vectors bits Mask */ - -#define FPU_MVFR0_Square_root_Pos 20 /*!< MVFR0: Square root bits Position */ -#define FPU_MVFR0_Square_root_Msk (0xFUL << FPU_MVFR0_Square_root_Pos) /*!< MVFR0: Square root bits Mask */ - -#define FPU_MVFR0_Divide_Pos 16 /*!< MVFR0: Divide bits Position */ -#define FPU_MVFR0_Divide_Msk (0xFUL << FPU_MVFR0_Divide_Pos) /*!< MVFR0: Divide bits Mask */ - -#define FPU_MVFR0_FP_excep_trapping_Pos 12 /*!< MVFR0: FP exception trapping bits Position */ -#define FPU_MVFR0_FP_excep_trapping_Msk (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos) /*!< MVFR0: FP exception trapping bits Mask */ - -#define FPU_MVFR0_Double_precision_Pos 8 /*!< MVFR0: Double-precision bits Position */ -#define FPU_MVFR0_Double_precision_Msk (0xFUL << FPU_MVFR0_Double_precision_Pos) /*!< MVFR0: Double-precision bits Mask */ - -#define FPU_MVFR0_Single_precision_Pos 4 /*!< MVFR0: Single-precision bits Position */ -#define FPU_MVFR0_Single_precision_Msk (0xFUL << FPU_MVFR0_Single_precision_Pos) /*!< MVFR0: Single-precision bits Mask */ - -#define FPU_MVFR0_A_SIMD_registers_Pos 0 /*!< MVFR0: A_SIMD registers bits Position */ -#define FPU_MVFR0_A_SIMD_registers_Msk (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos) /*!< MVFR0: A_SIMD registers bits Mask */ - -/* Media and FP Feature Register 1 */ -#define FPU_MVFR1_FP_fused_MAC_Pos 28 /*!< MVFR1: FP fused MAC bits Position */ -#define FPU_MVFR1_FP_fused_MAC_Msk (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos) /*!< MVFR1: FP fused MAC bits Mask */ - -#define FPU_MVFR1_FP_HPFP_Pos 24 /*!< MVFR1: FP HPFP bits Position */ -#define FPU_MVFR1_FP_HPFP_Msk (0xFUL << FPU_MVFR1_FP_HPFP_Pos) /*!< MVFR1: FP HPFP bits Mask */ - -#define FPU_MVFR1_D_NaN_mode_Pos 4 /*!< MVFR1: D_NaN mode bits Position */ -#define FPU_MVFR1_D_NaN_mode_Msk (0xFUL << FPU_MVFR1_D_NaN_mode_Pos) /*!< MVFR1: D_NaN mode bits Mask */ - -#define FPU_MVFR1_FtZ_mode_Pos 0 /*!< MVFR1: FtZ mode bits Position */ -#define FPU_MVFR1_FtZ_mode_Msk (0xFUL << FPU_MVFR1_FtZ_mode_Pos) /*!< MVFR1: FtZ mode bits Mask */ - -/*@} end of group CMSIS_FPU */ -#endif - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug) - \brief Type definitions for the Core Debug Registers - @{ - */ - -/** \brief Structure type to access the Core Debug Register (CoreDebug). - */ -typedef struct -{ - __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */ - __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */ - __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */ - __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */ -} CoreDebug_Type; - -/* Debug Halting Control and Status Register */ -#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */ -#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */ - -#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */ -#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */ - -#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */ -#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */ - -#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */ -#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */ - -#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */ -#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */ - -#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */ -#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */ - -#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */ -#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */ - -#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */ -#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */ - -#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */ -#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */ - -#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */ -#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */ - -#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */ -#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */ - -#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */ -#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */ - -/* Debug Core Register Selector Register */ -#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */ -#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */ - -#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */ -#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */ - -/* Debug Exception and Monitor Control Register */ -#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */ -#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */ - -#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */ -#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */ - -#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */ -#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */ - -#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */ -#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */ - -#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */ -#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */ - -#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */ -#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */ - -#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */ -#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */ - -#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */ -#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */ - -#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */ -#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */ - -#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */ -#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */ - -#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */ -#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */ - -#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */ -#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */ - -#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */ -#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */ - -/*@} end of group CMSIS_CoreDebug */ - - -/** \ingroup CMSIS_core_register - \defgroup CMSIS_core_base Core Definitions - \brief Definitions for base addresses, unions, and structures. - @{ - */ - -/* Memory mapping of Cortex-M4 Hardware */ -#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */ -#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */ -#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */ -#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */ -#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */ -#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */ -#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */ -#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */ - -#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */ -#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */ -#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */ -#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */ -#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */ -#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */ -#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */ -#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */ - -#if (__MPU_PRESENT == 1) - #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */ - #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */ -#endif - -#if (__FPU_PRESENT == 1) - #define FPU_BASE (SCS_BASE + 0x0F30UL) /*!< Floating Point Unit */ - #define FPU ((FPU_Type *) FPU_BASE ) /*!< Floating Point Unit */ -#endif - -/*@} */ - - - -/******************************************************************************* - * Hardware Abstraction Layer - Core Function Interface contains: - - Core NVIC Functions - - Core SysTick Functions - - Core Debug Functions - - Core Register Access Functions - ******************************************************************************/ -/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference -*/ - - - -/* ########################## NVIC functions #################################### */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_NVICFunctions NVIC Functions - \brief Functions that manage interrupts and exceptions via the NVIC. - @{ - */ - -/** \brief Set Priority Grouping - - The function sets the priority grouping field using the required unlock sequence. - The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field. - Only values from 0..7 are used. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set. - - \param [in] PriorityGroup Priority grouping field. - */ -__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) -{ - uint32_t reg_value; - uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */ - - reg_value = SCB->AIRCR; /* read old register configuration */ - reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */ - reg_value = (reg_value | - ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) | - (PriorityGroupTmp << 8)); /* Insert write key and priorty group */ - SCB->AIRCR = reg_value; -} - - -/** \brief Get Priority Grouping - - The function reads the priority grouping field from the NVIC Interrupt Controller. - - \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field). - */ -__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void) -{ - return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */ -} - - -/** \brief Enable External Interrupt - - The function enables a device-specific interrupt in the NVIC interrupt controller. - - \param [in] IRQn External interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn) -{ -/* NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); enable interrupt */ - NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */ -} - - -/** \brief Disable External Interrupt - - The function disables a device-specific interrupt in the NVIC interrupt controller. - - \param [in] IRQn External interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn) -{ - NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */ -} - - -/** \brief Get Pending Interrupt - - The function reads the pending register in the NVIC and returns the pending bit - for the specified interrupt. - - \param [in] IRQn Interrupt number. - - \return 0 Interrupt status is not pending. - \return 1 Interrupt status is pending. - */ -__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn) -{ - return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */ -} - - -/** \brief Set Pending Interrupt - - The function sets the pending bit of an external interrupt. - - \param [in] IRQn Interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn) -{ - NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */ -} - - -/** \brief Clear Pending Interrupt - - The function clears the pending bit of an external interrupt. - - \param [in] IRQn External interrupt number. Value cannot be negative. - */ -__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn) -{ - NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */ -} - - -/** \brief Get Active Interrupt - - The function reads the active register in NVIC and returns the active bit. - - \param [in] IRQn Interrupt number. - - \return 0 Interrupt status is not active. - \return 1 Interrupt status is active. - */ -__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn) -{ - return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */ -} - - -/** \brief Set Interrupt Priority - - The function sets the priority of an interrupt. - - \note The priority cannot be set for every core interrupt. - - \param [in] IRQn Interrupt number. - \param [in] priority Priority to set. - */ -__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority) -{ - if(IRQn < 0) { - SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */ - else { - NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */ -} - - -/** \brief Get Interrupt Priority - - The function reads the priority of an interrupt. The interrupt - number can be positive to specify an external (device specific) - interrupt, or negative to specify an internal (core) interrupt. - - - \param [in] IRQn Interrupt number. - \return Interrupt Priority. Value is aligned automatically to the implemented - priority bits of the microcontroller. - */ -__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn) -{ - - if(IRQn < 0) { - return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */ - else { - return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */ -} - - -/** \brief Encode Priority - - The function encodes the priority for an interrupt with the given priority group, - preemptive priority value, and subpriority value. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set. - - \param [in] PriorityGroup Used priority group. - \param [in] PreemptPriority Preemptive priority value (starting from 0). - \param [in] SubPriority Subpriority value (starting from 0). - \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority(). - */ -__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority) -{ - uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ - uint32_t PreemptPriorityBits; - uint32_t SubPriorityBits; - - PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; - SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; - - return ( - ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) | - ((SubPriority & ((1 << (SubPriorityBits )) - 1))) - ); -} - - -/** \brief Decode Priority - - The function decodes an interrupt priority value with a given priority group to - preemptive priority value and subpriority value. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set. - - \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority(). - \param [in] PriorityGroup Used priority group. - \param [out] pPreemptPriority Preemptive priority value (starting from 0). - \param [out] pSubPriority Subpriority value (starting from 0). - */ -__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority) -{ - uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */ - uint32_t PreemptPriorityBits; - uint32_t SubPriorityBits; - - PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp; - SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS; - - *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1); - *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1); -} - - -/** \brief System Reset - - The function initiates a system reset request to reset the MCU. - */ -__STATIC_INLINE void NVIC_SystemReset(void) -{ - __DSB(); /* Ensure all outstanding memory accesses included - buffered write are completed before reset */ - SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) | - (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | - SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */ - __DSB(); /* Ensure completion of memory access */ - while(1); /* wait until reset */ -} - -/*@} end of CMSIS_Core_NVICFunctions */ - - - -/* ################################## SysTick function ############################################ */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_SysTickFunctions SysTick Functions - \brief Functions that configure the System. - @{ - */ - -#if (__Vendor_SysTickConfig == 0) - -/** \brief System Tick Configuration - - The function initializes the System Timer and its interrupt, and starts the System Tick Timer. - Counter is in free running mode to generate periodic interrupts. - - \param [in] ticks Number of ticks between two interrupts. - - \return 0 Function succeeded. - \return 1 Function failed. - - \note When the variable __Vendor_SysTickConfig is set to 1, then the - function SysTick_Config is not included. In this case, the file device.h - must contain a vendor-specific implementation of this function. - - */ -__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks) -{ - if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */ - - SysTick->LOAD = ticks - 1; /* set reload register */ - NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */ - SysTick->VAL = 0; /* Load the SysTick Counter Value */ - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | - SysTick_CTRL_TICKINT_Msk | - SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */ - return (0); /* Function successful */ -} - -#endif - -/*@} end of CMSIS_Core_SysTickFunctions */ - - - -/* ##################################### Debug In/Output function ########################################### */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_core_DebugFunctions ITM Functions - \brief Functions that access the ITM debug interface. - @{ - */ - -extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */ -#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */ - - -/** \brief ITM Send Character - - The function transmits a character via the ITM channel 0, and - \li Just returns when no debugger is connected that has booked the output. - \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted. - - \param [in] ch Character to transmit. - - \returns Character to transmit. - */ -__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch) -{ - if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */ - (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */ - { - while (ITM->PORT[0].u32 == 0); - ITM->PORT[0].u8 = (uint8_t) ch; - } - return (ch); -} - - -/** \brief ITM Receive Character - - The function inputs a character via the external variable \ref ITM_RxBuffer. - - \return Received character. - \return -1 No character pending. - */ -__STATIC_INLINE int32_t ITM_ReceiveChar (void) { - int32_t ch = -1; /* no character available */ - - if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) { - ch = ITM_RxBuffer; - ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */ - } - - return (ch); -} - - -/** \brief ITM Check Character - - The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer. - - \return 0 No character available. - \return 1 Character available. - */ -__STATIC_INLINE int32_t ITM_CheckChar (void) { - - if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) { - return (0); /* no character available */ - } else { - return (1); /* character available */ - } -} - -/*@} end of CMSIS_core_DebugFunctions */ - -#endif /* __CORE_CM4_H_DEPENDANT */ - -#endif /* __CMSIS_GENERIC */ - -#ifdef __cplusplus -} -#endif diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h deleted file mode 100644 index af1831ee1..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h +++ /dev/null @@ -1,673 +0,0 @@ -/**************************************************************************//** - * @file core_cm4_simd.h - * @brief CMSIS Cortex-M4 SIMD Header File - * @version V3.20 - * @date 25. February 2013 - * - * @note - * - ******************************************************************************/ -/* Copyright (c) 2009 - 2013 ARM LIMITED - - All rights reserved. - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 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. - - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. - ---------------------------------------------------------------------------*/ - - -#ifdef __cplusplus - extern "C" { -#endif - -#ifndef __CORE_CM4_SIMD_H -#define __CORE_CM4_SIMD_H - - -/******************************************************************************* - * Hardware Abstraction Layer - ******************************************************************************/ - - -/* ################### Compiler specific Intrinsics ########################### */ -/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics - Access to dedicated SIMD instructions - @{ -*/ - -#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ -/* ARM armcc specific functions */ - -/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ -#define __SADD8 __sadd8 -#define __QADD8 __qadd8 -#define __SHADD8 __shadd8 -#define __UADD8 __uadd8 -#define __UQADD8 __uqadd8 -#define __UHADD8 __uhadd8 -#define __SSUB8 __ssub8 -#define __QSUB8 __qsub8 -#define __SHSUB8 __shsub8 -#define __USUB8 __usub8 -#define __UQSUB8 __uqsub8 -#define __UHSUB8 __uhsub8 -#define __SADD16 __sadd16 -#define __QADD16 __qadd16 -#define __SHADD16 __shadd16 -#define __UADD16 __uadd16 -#define __UQADD16 __uqadd16 -#define __UHADD16 __uhadd16 -#define __SSUB16 __ssub16 -#define __QSUB16 __qsub16 -#define __SHSUB16 __shsub16 -#define __USUB16 __usub16 -#define __UQSUB16 __uqsub16 -#define __UHSUB16 __uhsub16 -#define __SASX __sasx -#define __QASX __qasx -#define __SHASX __shasx -#define __UASX __uasx -#define __UQASX __uqasx -#define __UHASX __uhasx -#define __SSAX __ssax -#define __QSAX __qsax -#define __SHSAX __shsax -#define __USAX __usax -#define __UQSAX __uqsax -#define __UHSAX __uhsax -#define __USAD8 __usad8 -#define __USADA8 __usada8 -#define __SSAT16 __ssat16 -#define __USAT16 __usat16 -#define __UXTB16 __uxtb16 -#define __UXTAB16 __uxtab16 -#define __SXTB16 __sxtb16 -#define __SXTAB16 __sxtab16 -#define __SMUAD __smuad -#define __SMUADX __smuadx -#define __SMLAD __smlad -#define __SMLADX __smladx -#define __SMLALD __smlald -#define __SMLALDX __smlaldx -#define __SMUSD __smusd -#define __SMUSDX __smusdx -#define __SMLSD __smlsd -#define __SMLSDX __smlsdx -#define __SMLSLD __smlsld -#define __SMLSLDX __smlsldx -#define __SEL __sel -#define __QADD __qadd -#define __QSUB __qsub - -#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \ - ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) ) - -#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \ - ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) ) - -#define __SMMLA(ARG1,ARG2,ARG3) ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \ - ((int64_t)(ARG3) << 32) ) >> 32)) - -/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ - - - -#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/ -/* IAR iccarm specific functions */ - -/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ -#include - -/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ - - - -#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/ -/* TI CCS specific functions */ - -/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ -#include - -/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ - - - -#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/ -/* GNU gcc specific functions */ - -/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3) -{ - uint32_t result; - - __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); - return(result); -} - -#define __SSAT16(ARG1,ARG2) \ -({ \ - uint32_t __RES, __ARG1 = (ARG1); \ - __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ - __RES; \ - }) - -#define __USAT16(ARG1,ARG2) \ -({ \ - uint32_t __RES, __ARG1 = (ARG1); \ - __ASM ("usat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ - __RES; \ - }) - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1) -{ - uint32_t result; - - __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1)); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1) -{ - uint32_t result; - - __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1)); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD (uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3) -{ - uint32_t result; - - __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3) -{ - uint32_t result; - - __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); - return(result); -} - -#define __SMLALD(ARG1,ARG2,ARG3) \ -({ \ - uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \ - __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ - (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ - }) - -#define __SMLALDX(ARG1,ARG2,ARG3) \ -({ \ - uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \ - __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ - (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ - }) - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD (uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3) -{ - uint32_t result; - - __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3) -{ - uint32_t result; - - __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) ); - return(result); -} - -#define __SMLSLD(ARG1,ARG2,ARG3) \ -({ \ - uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \ - __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ - (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ - }) - -#define __SMLSLDX(ARG1,ARG2,ARG3) \ -({ \ - uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \ - __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \ - (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \ - }) - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL (uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2) -{ - uint32_t result; - - __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) ); - return(result); -} - -#define __PKHBT(ARG1,ARG2,ARG3) \ -({ \ - uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \ - __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \ - __RES; \ - }) - -#define __PKHTB(ARG1,ARG2,ARG3) \ -({ \ - uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \ - if (ARG3 == 0) \ - __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2) ); \ - else \ - __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \ - __RES; \ - }) - -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3) -{ - int32_t result; - - __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) ); - return(result); -} - -/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ - - - -#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/ -/* TASKING carm specific functions */ - - -/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/ -/* not yet supported */ -/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/ - - -#endif - -/*@} end of group CMSIS_SIMD_intrinsics */ - - -#endif /* __CORE_CM4_SIMD_H */ - -#ifdef __cplusplus -} -#endif diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h deleted file mode 100644 index 139bc3c5e..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h +++ /dev/null @@ -1,636 +0,0 @@ -/**************************************************************************//** - * @file core_cmFunc.h - * @brief CMSIS Cortex-M Core Function Access Header File - * @version V3.20 - * @date 25. February 2013 - * - * @note - * - ******************************************************************************/ -/* Copyright (c) 2009 - 2013 ARM LIMITED - - All rights reserved. - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 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. - - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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 __CORE_CMFUNC_H -#define __CORE_CMFUNC_H - - -/* ########################### Core Function Access ########################### */ -/** \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions - @{ - */ - -#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ -/* ARM armcc specific functions */ - -#if (__ARMCC_VERSION < 400677) - #error "Please use ARM Compiler Toolchain V4.0.677 or later!" -#endif - -/* intrinsic void __enable_irq(); */ -/* intrinsic void __disable_irq(); */ - -/** \brief Get Control Register - - This function returns the content of the Control Register. - - \return Control Register value - */ -__STATIC_INLINE uint32_t __get_CONTROL(void) -{ - register uint32_t __regControl __ASM("control"); - return(__regControl); -} - - -/** \brief Set Control Register - - This function writes the given value to the Control Register. - - \param [in] control Control Register value to set - */ -__STATIC_INLINE void __set_CONTROL(uint32_t control) -{ - register uint32_t __regControl __ASM("control"); - __regControl = control; -} - - -/** \brief Get IPSR Register - - This function returns the content of the IPSR Register. - - \return IPSR Register value - */ -__STATIC_INLINE uint32_t __get_IPSR(void) -{ - register uint32_t __regIPSR __ASM("ipsr"); - return(__regIPSR); -} - - -/** \brief Get APSR Register - - This function returns the content of the APSR Register. - - \return APSR Register value - */ -__STATIC_INLINE uint32_t __get_APSR(void) -{ - register uint32_t __regAPSR __ASM("apsr"); - return(__regAPSR); -} - - -/** \brief Get xPSR Register - - This function returns the content of the xPSR Register. - - \return xPSR Register value - */ -__STATIC_INLINE uint32_t __get_xPSR(void) -{ - register uint32_t __regXPSR __ASM("xpsr"); - return(__regXPSR); -} - - -/** \brief Get Process Stack Pointer - - This function returns the current value of the Process Stack Pointer (PSP). - - \return PSP Register value - */ -__STATIC_INLINE uint32_t __get_PSP(void) -{ - register uint32_t __regProcessStackPointer __ASM("psp"); - return(__regProcessStackPointer); -} - - -/** \brief Set Process Stack Pointer - - This function assigns the given value to the Process Stack Pointer (PSP). - - \param [in] topOfProcStack Process Stack Pointer value to set - */ -__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack) -{ - register uint32_t __regProcessStackPointer __ASM("psp"); - __regProcessStackPointer = topOfProcStack; -} - - -/** \brief Get Main Stack Pointer - - This function returns the current value of the Main Stack Pointer (MSP). - - \return MSP Register value - */ -__STATIC_INLINE uint32_t __get_MSP(void) -{ - register uint32_t __regMainStackPointer __ASM("msp"); - return(__regMainStackPointer); -} - - -/** \brief Set Main Stack Pointer - - This function assigns the given value to the Main Stack Pointer (MSP). - - \param [in] topOfMainStack Main Stack Pointer value to set - */ -__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack) -{ - register uint32_t __regMainStackPointer __ASM("msp"); - __regMainStackPointer = topOfMainStack; -} - - -/** \brief Get Priority Mask - - This function returns the current state of the priority mask bit from the Priority Mask Register. - - \return Priority Mask value - */ -__STATIC_INLINE uint32_t __get_PRIMASK(void) -{ - register uint32_t __regPriMask __ASM("primask"); - return(__regPriMask); -} - - -/** \brief Set Priority Mask - - This function assigns the given value to the Priority Mask Register. - - \param [in] priMask Priority Mask - */ -__STATIC_INLINE void __set_PRIMASK(uint32_t priMask) -{ - register uint32_t __regPriMask __ASM("primask"); - __regPriMask = (priMask); -} - - -#if (__CORTEX_M >= 0x03) - -/** \brief Enable FIQ - - This function enables FIQ interrupts by clearing the F-bit in the CPSR. - Can only be executed in Privileged modes. - */ -#define __enable_fault_irq __enable_fiq - - -/** \brief Disable FIQ - - This function disables FIQ interrupts by setting the F-bit in the CPSR. - Can only be executed in Privileged modes. - */ -#define __disable_fault_irq __disable_fiq - - -/** \brief Get Base Priority - - This function returns the current value of the Base Priority register. - - \return Base Priority register value - */ -__STATIC_INLINE uint32_t __get_BASEPRI(void) -{ - register uint32_t __regBasePri __ASM("basepri"); - return(__regBasePri); -} - - -/** \brief Set Base Priority - - This function assigns the given value to the Base Priority register. - - \param [in] basePri Base Priority value to set - */ -__STATIC_INLINE void __set_BASEPRI(uint32_t basePri) -{ - register uint32_t __regBasePri __ASM("basepri"); - __regBasePri = (basePri & 0xff); -} - - -/** \brief Get Fault Mask - - This function returns the current value of the Fault Mask register. - - \return Fault Mask register value - */ -__STATIC_INLINE uint32_t __get_FAULTMASK(void) -{ - register uint32_t __regFaultMask __ASM("faultmask"); - return(__regFaultMask); -} - - -/** \brief Set Fault Mask - - This function assigns the given value to the Fault Mask register. - - \param [in] faultMask Fault Mask value to set - */ -__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask) -{ - register uint32_t __regFaultMask __ASM("faultmask"); - __regFaultMask = (faultMask & (uint32_t)1); -} - -#endif /* (__CORTEX_M >= 0x03) */ - - -#if (__CORTEX_M == 0x04) - -/** \brief Get FPSCR - - This function returns the current value of the Floating Point Status/Control register. - - \return Floating Point Status/Control register value - */ -__STATIC_INLINE uint32_t __get_FPSCR(void) -{ -#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - register uint32_t __regfpscr __ASM("fpscr"); - return(__regfpscr); -#else - return(0); -#endif -} - - -/** \brief Set FPSCR - - This function assigns the given value to the Floating Point Status/Control register. - - \param [in] fpscr Floating Point Status/Control value to set - */ -__STATIC_INLINE void __set_FPSCR(uint32_t fpscr) -{ -#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - register uint32_t __regfpscr __ASM("fpscr"); - __regfpscr = (fpscr); -#endif -} - -#endif /* (__CORTEX_M == 0x04) */ - - -#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/ -/* IAR iccarm specific functions */ - -#include - - -#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/ -/* TI CCS specific functions */ - -#include - - -#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/ -/* GNU gcc specific functions */ - -/** \brief Enable IRQ Interrupts - - This function enables IRQ interrupts by clearing the I-bit in the CPSR. - Can only be executed in Privileged modes. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void) -{ - __ASM volatile ("cpsie i" : : : "memory"); -} - - -/** \brief Disable IRQ Interrupts - - This function disables IRQ interrupts by setting the I-bit in the CPSR. - Can only be executed in Privileged modes. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void) -{ - __ASM volatile ("cpsid i" : : : "memory"); -} - - -/** \brief Get Control Register - - This function returns the content of the Control Register. - - \return Control Register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, control" : "=r" (result) ); - return(result); -} - - -/** \brief Set Control Register - - This function writes the given value to the Control Register. - - \param [in] control Control Register value to set - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control) -{ - __ASM volatile ("MSR control, %0" : : "r" (control) : "memory"); -} - - -/** \brief Get IPSR Register - - This function returns the content of the IPSR Register. - - \return IPSR Register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, ipsr" : "=r" (result) ); - return(result); -} - - -/** \brief Get APSR Register - - This function returns the content of the APSR Register. - - \return APSR Register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, apsr" : "=r" (result) ); - return(result); -} - - -/** \brief Get xPSR Register - - This function returns the content of the xPSR Register. - - \return xPSR Register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, xpsr" : "=r" (result) ); - return(result); -} - - -/** \brief Get Process Stack Pointer - - This function returns the current value of the Process Stack Pointer (PSP). - - \return PSP Register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void) -{ - register uint32_t result; - - __ASM volatile ("MRS %0, psp\n" : "=r" (result) ); - return(result); -} - - -/** \brief Set Process Stack Pointer - - This function assigns the given value to the Process Stack Pointer (PSP). - - \param [in] topOfProcStack Process Stack Pointer value to set - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack) -{ - __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp"); -} - - -/** \brief Get Main Stack Pointer - - This function returns the current value of the Main Stack Pointer (MSP). - - \return MSP Register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void) -{ - register uint32_t result; - - __ASM volatile ("MRS %0, msp\n" : "=r" (result) ); - return(result); -} - - -/** \brief Set Main Stack Pointer - - This function assigns the given value to the Main Stack Pointer (MSP). - - \param [in] topOfMainStack Main Stack Pointer value to set - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack) -{ - __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp"); -} - - -/** \brief Get Priority Mask - - This function returns the current state of the priority mask bit from the Priority Mask Register. - - \return Priority Mask value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, primask" : "=r" (result) ); - return(result); -} - - -/** \brief Set Priority Mask - - This function assigns the given value to the Priority Mask Register. - - \param [in] priMask Priority Mask - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask) -{ - __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory"); -} - - -#if (__CORTEX_M >= 0x03) - -/** \brief Enable FIQ - - This function enables FIQ interrupts by clearing the F-bit in the CPSR. - Can only be executed in Privileged modes. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void) -{ - __ASM volatile ("cpsie f" : : : "memory"); -} - - -/** \brief Disable FIQ - - This function disables FIQ interrupts by setting the F-bit in the CPSR. - Can only be executed in Privileged modes. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void) -{ - __ASM volatile ("cpsid f" : : : "memory"); -} - - -/** \brief Get Base Priority - - This function returns the current value of the Base Priority register. - - \return Base Priority register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, basepri_max" : "=r" (result) ); - return(result); -} - - -/** \brief Set Base Priority - - This function assigns the given value to the Base Priority register. - - \param [in] basePri Base Priority value to set - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value) -{ - __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory"); -} - - -/** \brief Get Fault Mask - - This function returns the current value of the Fault Mask register. - - \return Fault Mask register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void) -{ - uint32_t result; - - __ASM volatile ("MRS %0, faultmask" : "=r" (result) ); - return(result); -} - - -/** \brief Set Fault Mask - - This function assigns the given value to the Fault Mask register. - - \param [in] faultMask Fault Mask value to set - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask) -{ - __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory"); -} - -#endif /* (__CORTEX_M >= 0x03) */ - - -#if (__CORTEX_M == 0x04) - -/** \brief Get FPSCR - - This function returns the current value of the Floating Point Status/Control register. - - \return Floating Point Status/Control register value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void) -{ -#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - uint32_t result; - - /* Empty asm statement works as a scheduling barrier */ - __ASM volatile (""); - __ASM volatile ("VMRS %0, fpscr" : "=r" (result) ); - __ASM volatile (""); - return(result); -#else - return(0); -#endif -} - - -/** \brief Set FPSCR - - This function assigns the given value to the Floating Point Status/Control register. - - \param [in] fpscr Floating Point Status/Control value to set - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr) -{ -#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - /* Empty asm statement works as a scheduling barrier */ - __ASM volatile (""); - __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc"); - __ASM volatile (""); -#endif -} - -#endif /* (__CORTEX_M == 0x04) */ - - -#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/ -/* TASKING carm specific functions */ - -/* - * The CMSIS functions have been implemented as intrinsics in the compiler. - * Please use "carm -?i" to get an up to date list of all instrinsics, - * Including the CMSIS ones. - */ - -#endif - -/*@} end of CMSIS_Core_RegAccFunctions */ - - -#endif /* __CORE_CMFUNC_H */ diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h deleted file mode 100644 index 8946c2c49..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h +++ /dev/null @@ -1,688 +0,0 @@ -/**************************************************************************//** - * @file core_cmInstr.h - * @brief CMSIS Cortex-M Core Instruction Access Header File - * @version V3.20 - * @date 05. March 2013 - * - * @note - * - ******************************************************************************/ -/* Copyright (c) 2009 - 2013 ARM LIMITED - - All rights reserved. - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 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. - - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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 __CORE_CMINSTR_H -#define __CORE_CMINSTR_H - - -/* ########################## Core Instruction Access ######################### */ -/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface - Access to dedicated instructions - @{ -*/ - -#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/ -/* ARM armcc specific functions */ - -#if (__ARMCC_VERSION < 400677) - #error "Please use ARM Compiler Toolchain V4.0.677 or later!" -#endif - - -/** \brief No Operation - - No Operation does nothing. This instruction can be used for code alignment purposes. - */ -#define __NOP __nop - - -/** \brief Wait For Interrupt - - Wait For Interrupt is a hint instruction that suspends execution - until one of a number of events occurs. - */ -#define __WFI __wfi - - -/** \brief Wait For Event - - Wait For Event is a hint instruction that permits the processor to enter - a low-power state until one of a number of events occurs. - */ -#define __WFE __wfe - - -/** \brief Send Event - - Send Event is a hint instruction. It causes an event to be signaled to the CPU. - */ -#define __SEV __sev - - -/** \brief Instruction Synchronization Barrier - - Instruction Synchronization Barrier flushes the pipeline in the processor, - so that all instructions following the ISB are fetched from cache or - memory, after the instruction has been completed. - */ -#define __ISB() __isb(0xF) - - -/** \brief Data Synchronization Barrier - - This function acts as a special kind of Data Memory Barrier. - It completes when all explicit memory accesses before this instruction complete. - */ -#define __DSB() __dsb(0xF) - - -/** \brief Data Memory Barrier - - This function ensures the apparent order of the explicit memory operations before - and after the instruction, without ensuring their completion. - */ -#define __DMB() __dmb(0xF) - - -/** \brief Reverse byte order (32 bit) - - This function reverses the byte order in integer value. - - \param [in] value Value to reverse - \return Reversed value - */ -#define __REV __rev - - -/** \brief Reverse byte order (16 bit) - - This function reverses the byte order in two unsigned short values. - - \param [in] value Value to reverse - \return Reversed value - */ -#ifndef __NO_EMBEDDED_ASM -__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value) -{ - rev16 r0, r0 - bx lr -} -#endif - -/** \brief Reverse byte order in signed short value - - This function reverses the byte order in a signed short value with sign extension to integer. - - \param [in] value Value to reverse - \return Reversed value - */ -#ifndef __NO_EMBEDDED_ASM -__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value) -{ - revsh r0, r0 - bx lr -} -#endif - - -/** \brief Rotate Right in unsigned value (32 bit) - - This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits. - - \param [in] value Value to rotate - \param [in] value Number of Bits to rotate - \return Rotated value - */ -#define __ROR __ror - - -/** \brief Breakpoint - - This function causes the processor to enter Debug state. - Debug tools can use this to investigate system state when the instruction at a particular address is reached. - - \param [in] value is ignored by the processor. - If required, a debugger can use it to store additional information about the breakpoint. - */ -#define __BKPT(value) __breakpoint(value) - - -#if (__CORTEX_M >= 0x03) - -/** \brief Reverse bit order of value - - This function reverses the bit order of the given value. - - \param [in] value Value to reverse - \return Reversed value - */ -#define __RBIT __rbit - - -/** \brief LDR Exclusive (8 bit) - - This function performs a exclusive LDR command for 8 bit value. - - \param [in] ptr Pointer to data - \return value of type uint8_t at (*ptr) - */ -#define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr)) - - -/** \brief LDR Exclusive (16 bit) - - This function performs a exclusive LDR command for 16 bit values. - - \param [in] ptr Pointer to data - \return value of type uint16_t at (*ptr) - */ -#define __LDREXH(ptr) ((uint16_t) __ldrex(ptr)) - - -/** \brief LDR Exclusive (32 bit) - - This function performs a exclusive LDR command for 32 bit values. - - \param [in] ptr Pointer to data - \return value of type uint32_t at (*ptr) - */ -#define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr)) - - -/** \brief STR Exclusive (8 bit) - - This function performs a exclusive STR command for 8 bit values. - - \param [in] value Value to store - \param [in] ptr Pointer to location - \return 0 Function succeeded - \return 1 Function failed - */ -#define __STREXB(value, ptr) __strex(value, ptr) - - -/** \brief STR Exclusive (16 bit) - - This function performs a exclusive STR command for 16 bit values. - - \param [in] value Value to store - \param [in] ptr Pointer to location - \return 0 Function succeeded - \return 1 Function failed - */ -#define __STREXH(value, ptr) __strex(value, ptr) - - -/** \brief STR Exclusive (32 bit) - - This function performs a exclusive STR command for 32 bit values. - - \param [in] value Value to store - \param [in] ptr Pointer to location - \return 0 Function succeeded - \return 1 Function failed - */ -#define __STREXW(value, ptr) __strex(value, ptr) - - -/** \brief Remove the exclusive lock - - This function removes the exclusive lock which is created by LDREX. - - */ -#define __CLREX __clrex - - -/** \brief Signed Saturate - - This function saturates a signed value. - - \param [in] value Value to be saturated - \param [in] sat Bit position to saturate to (1..32) - \return Saturated value - */ -#define __SSAT __ssat - - -/** \brief Unsigned Saturate - - This function saturates an unsigned value. - - \param [in] value Value to be saturated - \param [in] sat Bit position to saturate to (0..31) - \return Saturated value - */ -#define __USAT __usat - - -/** \brief Count leading zeros - - This function counts the number of leading zeros of a data value. - - \param [in] value Value to count the leading zeros - \return number of leading zeros in value - */ -#define __CLZ __clz - -#endif /* (__CORTEX_M >= 0x03) */ - - - -#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/ -/* IAR iccarm specific functions */ - -#include - - -#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/ -/* TI CCS specific functions */ - -#include - - -#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/ -/* GNU gcc specific functions */ - -/* Define macros for porting to both thumb1 and thumb2. - * For thumb1, use low register (r0-r7), specified by constrant "l" - * Otherwise, use general registers, specified by constrant "r" */ -#if defined (__thumb__) && !defined (__thumb2__) -#define __CMSIS_GCC_OUT_REG(r) "=l" (r) -#define __CMSIS_GCC_USE_REG(r) "l" (r) -#else -#define __CMSIS_GCC_OUT_REG(r) "=r" (r) -#define __CMSIS_GCC_USE_REG(r) "r" (r) -#endif - -/** \brief No Operation - - No Operation does nothing. This instruction can be used for code alignment purposes. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void) -{ - __ASM volatile ("nop"); -} - - -/** \brief Wait For Interrupt - - Wait For Interrupt is a hint instruction that suspends execution - until one of a number of events occurs. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void) -{ - __ASM volatile ("wfi"); -} - - -/** \brief Wait For Event - - Wait For Event is a hint instruction that permits the processor to enter - a low-power state until one of a number of events occurs. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void) -{ - __ASM volatile ("wfe"); -} - - -/** \brief Send Event - - Send Event is a hint instruction. It causes an event to be signaled to the CPU. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void) -{ - __ASM volatile ("sev"); -} - - -/** \brief Instruction Synchronization Barrier - - Instruction Synchronization Barrier flushes the pipeline in the processor, - so that all instructions following the ISB are fetched from cache or - memory, after the instruction has been completed. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void) -{ - __ASM volatile ("isb"); -} - - -/** \brief Data Synchronization Barrier - - This function acts as a special kind of Data Memory Barrier. - It completes when all explicit memory accesses before this instruction complete. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void) -{ - __ASM volatile ("dsb"); -} - - -/** \brief Data Memory Barrier - - This function ensures the apparent order of the explicit memory operations before - and after the instruction, without ensuring their completion. - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void) -{ - __ASM volatile ("dmb"); -} - - -/** \brief Reverse byte order (32 bit) - - This function reverses the byte order in integer value. - - \param [in] value Value to reverse - \return Reversed value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value) -{ -#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5) - return __builtin_bswap32(value); -#else - uint32_t result; - - __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); - return(result); -#endif -} - - -/** \brief Reverse byte order (16 bit) - - This function reverses the byte order in two unsigned short values. - - \param [in] value Value to reverse - \return Reversed value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value) -{ - uint32_t result; - - __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); - return(result); -} - - -/** \brief Reverse byte order in signed short value - - This function reverses the byte order in a signed short value with sign extension to integer. - - \param [in] value Value to reverse - \return Reversed value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value) -{ -#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) - return (short)__builtin_bswap16(value); -#else - uint32_t result; - - __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); - return(result); -#endif -} - - -/** \brief Rotate Right in unsigned value (32 bit) - - This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits. - - \param [in] value Value to rotate - \param [in] value Number of Bits to rotate - \return Rotated value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2) -{ - return (op1 >> op2) | (op1 << (32 - op2)); -} - - -/** \brief Breakpoint - - This function causes the processor to enter Debug state. - Debug tools can use this to investigate system state when the instruction at a particular address is reached. - - \param [in] value is ignored by the processor. - If required, a debugger can use it to store additional information about the breakpoint. - */ -#define __BKPT(value) __ASM volatile ("bkpt "#value) - - -#if (__CORTEX_M >= 0x03) - -/** \brief Reverse bit order of value - - This function reverses the bit order of the given value. - - \param [in] value Value to reverse - \return Reversed value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value) -{ - uint32_t result; - - __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) ); - return(result); -} - - -/** \brief LDR Exclusive (8 bit) - - This function performs a exclusive LDR command for 8 bit value. - - \param [in] ptr Pointer to data - \return value of type uint8_t at (*ptr) - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr) -{ - uint32_t result; - -#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) - __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) ); -#else - /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not - accepted by assembler. So has to use following less efficient pattern. - */ - __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" ); -#endif - return(result); -} - - -/** \brief LDR Exclusive (16 bit) - - This function performs a exclusive LDR command for 16 bit values. - - \param [in] ptr Pointer to data - \return value of type uint16_t at (*ptr) - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr) -{ - uint32_t result; - -#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) - __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) ); -#else - /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not - accepted by assembler. So has to use following less efficient pattern. - */ - __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" ); -#endif - return(result); -} - - -/** \brief LDR Exclusive (32 bit) - - This function performs a exclusive LDR command for 32 bit values. - - \param [in] ptr Pointer to data - \return value of type uint32_t at (*ptr) - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr) -{ - uint32_t result; - - __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) ); - return(result); -} - - -/** \brief STR Exclusive (8 bit) - - This function performs a exclusive STR command for 8 bit values. - - \param [in] value Value to store - \param [in] ptr Pointer to location - \return 0 Function succeeded - \return 1 Function failed - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr) -{ - uint32_t result; - - __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) ); - return(result); -} - - -/** \brief STR Exclusive (16 bit) - - This function performs a exclusive STR command for 16 bit values. - - \param [in] value Value to store - \param [in] ptr Pointer to location - \return 0 Function succeeded - \return 1 Function failed - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr) -{ - uint32_t result; - - __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) ); - return(result); -} - - -/** \brief STR Exclusive (32 bit) - - This function performs a exclusive STR command for 32 bit values. - - \param [in] value Value to store - \param [in] ptr Pointer to location - \return 0 Function succeeded - \return 1 Function failed - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr) -{ - uint32_t result; - - __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) ); - return(result); -} - - -/** \brief Remove the exclusive lock - - This function removes the exclusive lock which is created by LDREX. - - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void) -{ - __ASM volatile ("clrex" ::: "memory"); -} - - -/** \brief Signed Saturate - - This function saturates a signed value. - - \param [in] value Value to be saturated - \param [in] sat Bit position to saturate to (1..32) - \return Saturated value - */ -#define __SSAT(ARG1,ARG2) \ -({ \ - uint32_t __RES, __ARG1 = (ARG1); \ - __ASM ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ - __RES; \ - }) - - -/** \brief Unsigned Saturate - - This function saturates an unsigned value. - - \param [in] value Value to be saturated - \param [in] sat Bit position to saturate to (0..31) - \return Saturated value - */ -#define __USAT(ARG1,ARG2) \ -({ \ - uint32_t __RES, __ARG1 = (ARG1); \ - __ASM ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \ - __RES; \ - }) - - -/** \brief Count leading zeros - - This function counts the number of leading zeros of a data value. - - \param [in] value Value to count the leading zeros - \return number of leading zeros in value - */ -__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value) -{ - uint32_t result; - - __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) ); - return(result); -} - -#endif /* (__CORTEX_M >= 0x03) */ - - - - -#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/ -/* TASKING carm specific functions */ - -/* - * The CMSIS functions have been implemented as intrinsics in the compiler. - * Please use "carm -?i" to get an up to date list of all intrinsics, - * Including the CMSIS ones. - */ - -#endif - -/*@}*/ /* end of group CMSIS_Core_InstructionInterface */ - -#endif /* __CORE_CMINSTR_H */ diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a deleted file mode 100644 index 6898bc27d..000000000 Binary files a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a and /dev/null differ diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a deleted file mode 100755 index a0185eaa9..000000000 Binary files a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a and /dev/null differ diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a deleted file mode 100755 index 94525528e..000000000 Binary files a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a and /dev/null differ diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/modules/mathlib/CMSIS/library.mk deleted file mode 100644 index 0cc1b559d..000000000 --- a/src/modules/mathlib/CMSIS/library.mk +++ /dev/null @@ -1,46 +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. -# -############################################################################ - -# -# ARM CMSIS DSP library -# - -ifeq ($(CONFIG_ARCH),CORTEXM4F) -PREBUILT_LIB := libarm_cortexM4lf_math.a -else ifeq ($(CONFIG_ARCH),CORTEXM4) -PREBUILT_LIB := libarm_cortexM4l_math.a -else ifeq ($(CONFIG_ARCH),CORTEXM3) -PREBUILT_LIB := libarm_cortexM3l_math.a -else -$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library) -endif diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/modules/mathlib/CMSIS/license.txt deleted file mode 100644 index 31afac1ec..000000000 --- a/src/modules/mathlib/CMSIS/license.txt +++ /dev/null @@ -1,27 +0,0 @@ -All pre-built libraries are guided by the following license: - -Copyright (C) 2009-2012 ARM Limited. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - 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. - - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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. diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081..000000000 --- a/src/modules/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +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 Dcm.cpp - * - * math direction cosine matrix - */ - -#include - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3a..000000000 --- a/src/modules/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +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 Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb..000000000 --- a/src/modules/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +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 Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp deleted file mode 100644 index 399eecfa7..000000000 --- a/src/modules/mathlib/math/EulerAngles.hpp +++ /dev/null @@ -1,74 +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 Vector.h - * - * math vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class Quaternion; -class Dcm; - -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math - diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp deleted file mode 100644 index d4c892d8a..000000000 --- a/src/modules/mathlib/math/Limits.cpp +++ /dev/null @@ -1,146 +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 Limits.cpp - * - * Limiting / constrain helper functions - */ - - -#include -#include - -#include "Limits.hpp" - - -namespace math { - - -float __EXPORT min(float val1, float val2) -{ - return (val1 < val2) ? val1 : val2; -} - -int __EXPORT min(int val1, int val2) -{ - return (val1 < val2) ? val1 : val2; -} - -unsigned __EXPORT min(unsigned val1, unsigned val2) -{ - return (val1 < val2) ? val1 : val2; -} - -uint64_t __EXPORT min(uint64_t val1, uint64_t val2) -{ - return (val1 < val2) ? val1 : val2; -} - -double __EXPORT min(double val1, double val2) -{ - return (val1 < val2) ? val1 : val2; -} - -float __EXPORT max(float val1, float val2) -{ - return (val1 > val2) ? val1 : val2; -} - -int __EXPORT max(int val1, int val2) -{ - return (val1 > val2) ? val1 : val2; -} - -unsigned __EXPORT max(unsigned val1, unsigned val2) -{ - return (val1 > val2) ? val1 : val2; -} - -uint64_t __EXPORT max(uint64_t val1, uint64_t val2) -{ - return (val1 > val2) ? val1 : val2; -} - -double __EXPORT max(double val1, double val2) -{ - return (val1 > val2) ? val1 : val2; -} - - -float __EXPORT constrain(float val, float min, float max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -int __EXPORT constrain(int val, int min, int max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -double __EXPORT constrain(double val, double min, double max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -float __EXPORT radians(float degrees) -{ - return (degrees / 180.0f) * M_PI_F; -} - -double __EXPORT radians(double degrees) -{ - return (degrees / 180.0) * M_PI; -} - -float __EXPORT degrees(float radians) -{ - return (radians / M_PI_F) * 180.0f; -} - -double __EXPORT degrees(double radians) -{ - return (radians / M_PI) * 180.0; -} - -} \ No newline at end of file diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/mathlib/math/Limits.hpp deleted file mode 100644 index fb778dd66..000000000 --- a/src/modules/mathlib/math/Limits.hpp +++ /dev/null @@ -1,87 +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 Limits.hpp - * - * Limiting / constrain helper functions - */ - -#pragma once - -#include -#include - -namespace math { - - -float __EXPORT min(float val1, float val2); - -int __EXPORT min(int val1, int val2); - -unsigned __EXPORT min(unsigned val1, unsigned val2); - -uint64_t __EXPORT min(uint64_t val1, uint64_t val2); - -double __EXPORT min(double val1, double val2); - -float __EXPORT max(float val1, float val2); - -int __EXPORT max(int val1, int val2); - -unsigned __EXPORT max(unsigned val1, unsigned val2); - -uint64_t __EXPORT max(uint64_t val1, uint64_t val2); - -double __EXPORT max(double val1, double val2); - - -float __EXPORT constrain(float val, float min, float max); - -int __EXPORT constrain(int val, int min, int max); - -unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); - -uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max); - -double __EXPORT constrain(double val, double min, double max); - -float __EXPORT radians(float degrees); - -double __EXPORT radians(double degrees); - -float __EXPORT degrees(float radians); - -double __EXPORT degrees(double radians); - -} \ No newline at end of file diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3..000000000 --- a/src/modules/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +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 Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/modules/mathlib/math/Matrix.hpp deleted file mode 100644 index f19db15ec..000000000 --- a/src/modules/mathlib/math/Matrix.hpp +++ /dev/null @@ -1,61 +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 Matrix.h - * - * matrix code - */ - -#pragma once - -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif - -namespace math -{ -class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca6..000000000 --- a/src/modules/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +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 Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp deleted file mode 100644 index 048a55d33..000000000 --- a/src/modules/mathlib/math/Quaternion.hpp +++ /dev/null @@ -1,115 +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 Quaternion.hpp - * - * math quaternion lib - */ - -#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector -{ -public: - - /** - * default ctor - */ - Quaternion(); - - /** - * ctor from floats - */ - Quaternion(float a, float b, float c, float d); - - /** - * ctor from data - */ - Quaternion(const float *data); - - /** - * ctor from Vector - */ - Quaternion(const Vector &v); - - /** - * ctor from EulerAngles - */ - Quaternion(const EulerAngles &euler); - - /** - * ctor from Dcm - */ - Quaternion(const Dcm &dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); - - /** - * dtor - */ - virtual ~Quaternion(); - - /** - * derivative - */ - Vector derivative(const Vector &w); - - /** - * accessors - */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } -}; - -int __EXPORT quaternionTest(); -} // math - diff --git a/src/modules/mathlib/math/Vector.cpp b/src/modules/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396..000000000 --- a/src/modules/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +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 Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Vector.hpp b/src/modules/mathlib/math/Vector.hpp deleted file mode 100644 index 73de793d5..000000000 --- a/src/modules/mathlib/math/Vector.hpp +++ /dev/null @@ -1,57 +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 Vector.h - * - * math vector - */ - -#pragma once - -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif - -namespace math -{ -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817..000000000 --- a/src/modules/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +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 Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp deleted file mode 100644 index ecd62e81c..000000000 --- a/src/modules/mathlib/math/Vector2f.hpp +++ /dev/null @@ -1,79 +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 Vector2f.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math - diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600e..000000000 --- a/src/modules/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +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 Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp deleted file mode 100644 index 568d9669a..000000000 --- a/src/modules/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,76 +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 Vector3.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector3 : - public Vector -{ -public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ -}; - -int __EXPORT vector3Test(); -} // math - diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/modules/mathlib/math/arm/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/modules/mathlib/math/arm/Matrix.cpp +++ /dev/null @@ -1,40 +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 Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 715fd3a5e..000000000 --- a/src/modules/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +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 Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/modules/mathlib/math/arm/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/modules/mathlib/math/arm/Vector.cpp +++ /dev/null @@ -1,40 +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 Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 4155800e8..000000000 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +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 Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp deleted file mode 100644 index efb17225d..000000000 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/**************************************************************************** - * - * 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 LowPassFilter.cpp -/// @brief A class to implement a second order low pass filter -/// Author: Leonard Hall - -#include "LowPassFilter2p.hpp" -#include "math.h" - -namespace math -{ - -void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) -{ - _cutoff_freq = cutoff_freq; - float fr = sample_freq/_cutoff_freq; - float ohm = tanf(M_PI_F/fr); - float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; - _b0 = ohm*ohm/c; - _b1 = 2.0f*_b0; - _b2 = _b0; - _a1 = 2.0f*(ohm*ohm-1.0f)/c; - _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; -} - -float LowPassFilter2p::apply(float sample) -{ - // do the filtering - float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter - delay_element_0 = sample; - } - float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; - - _delay_element_2 = _delay_element_1; - _delay_element_1 = delay_element_0; - - // return the value. Should be no need to check limits - return output; -} - -} // namespace math - diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp deleted file mode 100644 index 208ec98d4..000000000 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/**************************************************************************** - * - * 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 LowPassFilter.h -/// @brief A class to implement a second order low pass filter -/// Author: Leonard Hall -/// Adapted for PX4 by Andrew Tridgell - -#pragma once - -namespace math -{ -class __EXPORT LowPassFilter2p -{ -public: - // constructor - LowPassFilter2p(float sample_freq, float cutoff_freq) { - // set initial parameters - set_cutoff_frequency(sample_freq, cutoff_freq); - _delay_element_1 = _delay_element_2 = 0; - } - - // change parameters - void set_cutoff_frequency(float sample_freq, float cutoff_freq); - - // apply - Add a new raw value to the filter - // and retrieve the filtered result - float apply(float sample); - - // return the cutoff frequency - float get_cutoff_freq(void) const { - return _cutoff_freq; - } - -private: - float _cutoff_freq; - float _a1; - float _a2; - float _b0; - float _b1; - float _b2; - float _delay_element_1; // buffered sample -1 - float _delay_element_2; // buffered sample -2 -}; - -} // namespace math diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk deleted file mode 100644 index fe92c8c70..000000000 --- a/src/modules/mathlib/math/filter/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# filter library -# -SRCS = LowPassFilter2p.cpp - -# -# In order to include .config we first have to save off the -# current makefile name, since app.mk needs it. -# -APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/modules/mathlib/math/generic/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/modules/mathlib/math/generic/Matrix.cpp +++ /dev/null @@ -1,40 +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 Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447..000000000 --- a/src/modules/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +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 Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/modules/mathlib/math/generic/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/modules/mathlib/math/generic/Vector.cpp +++ /dev/null @@ -1,40 +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 Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d..000000000 --- a/src/modules/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +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 Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf deleted file mode 100644 index eb67a4bfc..000000000 Binary files a/src/modules/mathlib/math/nasa_rotation_def.pdf and /dev/null differ diff --git a/src/modules/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp deleted file mode 100644 index 2fa2f7e7c..000000000 --- a/src/modules/mathlib/math/test/test.cpp +++ /dev/null @@ -1,94 +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 test.cpp - * - * Test library code - */ - -#include -#include -#include - -#include "test.hpp" - -bool __EXPORT equal(float a, float b, float epsilon) -{ - float diff = fabsf(a - b); - - if (diff > epsilon) { - printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); - return false; - - } else return true; -} - -void __EXPORT float2SigExp( - const float &num, - float &sig, - int &exp) -{ - if (isnan(num) || isinf(num)) { - sig = 0.0f; - exp = -99; - return; - } - - if (fabsf(num) < 1.0e-38f) { - sig = 0; - exp = 0; - return; - } - - exp = log10f(fabsf(num)); - - if (exp > 0) { - exp = ceil(exp); - - } else { - exp = floor(exp); - } - - sig = num; - - // cheap power since it is integer - if (exp > 0) { - for (int i = 0; i < abs(exp); i++) sig /= 10; - - } else { - for (int i = 0; i < abs(exp); i++) sig *= 10; - } -} - - diff --git a/src/modules/mathlib/math/test/test.hpp b/src/modules/mathlib/math/test/test.hpp deleted file mode 100644 index 2027bb827..000000000 --- a/src/modules/mathlib/math/test/test.hpp +++ /dev/null @@ -1,50 +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 test.hpp - * - * Controller library code - */ - -#pragma once - -//#include -//#include -//#include - -bool equal(float a, float b, float eps = 1e-5); -void float2SigExp( - const float &num, - float &sig, - int &exp); diff --git a/src/modules/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce deleted file mode 100644 index c3fba4729..000000000 --- a/src/modules/mathlib/math/test_math.sce +++ /dev/null @@ -1,63 +0,0 @@ -clc -clear -function out = float_truncate(in, digits) - out = round(in*10^digits) - out = out/10^digits -endfunction - -phi = 0.1 -theta = 0.2 -psi = 0.3 - -cosPhi = cos(phi) -cosPhi_2 = cos(phi/2) -sinPhi = sin(phi) -sinPhi_2 = sin(phi/2) - -cosTheta = cos(theta) -cosTheta_2 = cos(theta/2) -sinTheta = sin(theta) -sinTheta_2 = sin(theta/2) - -cosPsi = cos(psi) -cosPsi_2 = cos(psi/2) -sinPsi = sin(psi) -sinPsi_2 = sin(psi/2) - -C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; - cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; - -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] - -disp(C_nb) -//C_nb = float_truncate(C_nb,3) -//disp(C_nb) - -theta = asin(-C_nb(3,1)) -phi = atan(C_nb(3,2), C_nb(3,3)) -psi = atan(C_nb(2,1), C_nb(1,1)) -printf('phi %f\n', phi) -printf('theta %f\n', theta) -printf('psi %f\n', psi) - -q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; - sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; - cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; - cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] - -//q = float_truncate(q,3) - -a = q(1) -b = q(2) -c = q(3) -d = q(4) -printf('q: %f %f %f %f\n', a, b, c, d) -a2 = a*a -b2 = b*b -c2 = c*c -d2 = d*d - -C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); - 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); - 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] - -disp(C2_nb) diff --git a/src/modules/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h deleted file mode 100644 index 40ffb22bc..000000000 --- a/src/modules/mathlib/mathlib.h +++ /dev/null @@ -1,59 +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 mathlib.h - * - * Common header for mathlib exports. - */ - -#ifdef __cplusplus - -#pragma once - -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" -#include "math/Matrix.hpp" -#include "math/Quaternion.hpp" -#include "math/Vector.hpp" -#include "math/Vector3.hpp" -#include "math/Vector2f.hpp" -#include "math/Limits.hpp" - -#endif - -#ifdef CONFIG_ARCH_ARM - -#include "CMSIS/Include/arm_math.h" - -#endif \ No newline at end of file diff --git a/src/modules/mathlib/module.mk b/src/modules/mathlib/module.mk deleted file mode 100644 index 2146a1413..000000000 --- a/src/modules/mathlib/module.mk +++ /dev/null @@ -1,62 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Math library -# -SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ - math/Limits.cpp - -# -# In order to include .config we first have to save off the -# current makefile name, since app.mk needs it. -# -APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 3d3434670..f39bbaa72 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -279,6 +279,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..28f7af33c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -50,6 +50,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -101,6 +105,10 @@ static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_gyro = -1; +static orb_advert_t pub_hil_accel = -1; +static orb_advert_t pub_hil_mag = -1; +static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; @@ -412,12 +420,12 @@ handle_message(mavlink_message_t *msg) /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); float ias = calc_indicated_airspeed(imu.diff_pressure); // XXX need to fix this float tas = ias; + airspeed.timestamp = hrt_absolute_time(); airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; @@ -428,7 +436,67 @@ handle_message(mavlink_message_t *msg) } //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - /* publish */ + /* individual sensor publications */ + struct gyro_report gyro; + gyro.x_raw = imu.xgyro / mrad2rad; + gyro.y_raw = imu.ygyro / mrad2rad; + gyro.z_raw = imu.zgyro / mrad2rad; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + gyro.timestamp = hrt_absolute_time(); + + if (pub_hil_gyro < 0) { + pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { + orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + } + + struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); + + if (pub_hil_mag < 0) { + pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { + orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } + + struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); + + if (pub_hil_baro < 0) { + pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { + orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } + + /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); } else { @@ -552,6 +620,22 @@ handle_message(mavlink_message_t *msg) } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } + + struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d..c711b1803 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -138,6 +140,7 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -567,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -637,7 +666,7 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); float alt = global_pos.alt; float climb = global_pos.vz; @@ -773,7 +802,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 506f73105..e2e630046 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -80,6 +80,7 @@ struct mavlink_subscriptions { int safety_sub; int actuators_sub; int armed_sub; + int actuators_effective_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3466841c4..760e598bc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -64,9 +64,8 @@ #include #include #include -#include +#include #include -#include #include #include "position_estimator_inav_params.h" diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index e01cc4dda..310fbf60f 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -42,7 +42,7 @@ #include #include -#include "conversions.h" +#include #include "airspeed.h" @@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { - density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; -// printf("[airspeed] Invalid air density, using density at sea level\n"); + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; } float pressure_difference = total_pressure - static_pressure; - if(pressure_difference > 0) { + if (pressure_difference > 0) { return sqrtf((2.0f*(pressure_difference)) / density); - } else - { + } else { return -sqrtf((2.0f*fabsf(pressure_difference)) / density); } } + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index def53f0c1..8dccaab9c 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -85,6 +85,14 @@ */ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + /** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + __END_DECLS #endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c index ac94252c5..9105d83cb 100644 --- a/src/modules/systemlib/conversions.c +++ b/src/modules/systemlib/conversions.c @@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } - -void rot2quat(const float R[9], float Q[4]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - int32_t idx; - - /* conversion of rotation matrix to quaternion - * choose the largest component to begin with */ - q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; - q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; - q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; - q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; - - idx = 0; - - if (q0_2 < q1_2) { - q0_2 = q1_2; - - idx = 1; - } - - if (q0_2 < q2_2) { - q0_2 = q2_2; - idx = 2; - } - - if (q0_2 < q3_2) { - q0_2 = q3_2; - idx = 3; - } - - q0_2 = sqrtf(q0_2); - - /* solve for the remaining three components */ - if (idx == 0) { - q1_2 = q0_2; - q2_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[6] - R[2]) / 4.0F / q0_2; - q0_2 = (R[1] - R[3]) / 4.0F / q0_2; - - } else if (idx == 1) { - q2_2 = q0_2; - q1_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[6] + R[2]) / 4.0F / q0_2; - - } else if (idx == 2) { - q3_2 = q0_2; - q1_2 = (R[6] - R[2]) / 4.0F / q0_2; - q2_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[7] + R[5]) / 4.0F / q0_2; - - } else { - q1_2 = (R[1] - R[3]) / 4.0F / q0_2; - q2_2 = (R[6] + R[2]) / 4.0F / q0_2; - q3_2 = (R[7] + R[5]) / 4.0F / q0_2; - } - - /* return values */ - Q[0] = q1_2; - Q[1] = q2_2; - Q[2] = q3_2; - Q[3] = q0_2; -} - -void quat2rot(const float Q[4], float R[9]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - - memset(&R[0], 0, 9U * sizeof(float)); - - q0_2 = Q[0] * Q[0]; - q1_2 = Q[1] * Q[1]; - q2_2 = Q[2] * Q[2]; - q3_2 = Q[3] * Q[3]; - - R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; - R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); - R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); - R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); - R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; - R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); - R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); - R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); - R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; -} - -float get_air_density(float static_pressure, float temperature_celsius) -{ - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 064426f21..dc383e770 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,7 +43,6 @@ #define CONVERSIONS_H_ #include #include -#include __BEGIN_DECLS @@ -57,34 +56,6 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); -/** - * Converts a 3 x 3 rotation matrix to an unit quaternion. - * - * All orientations are expressed in NED frame. - * - * @param R rotation matrix to convert - * @param Q quaternion to write back to - */ -__EXPORT void rot2quat(const float R[9], float Q[4]); - -/** - * Converts an unit quaternion to a 3 x 3 rotation matrix. - * - * All orientations are expressed in NED frame. - * - * @param Q quaternion to convert - * @param R rotation matrix to write back to - */ -__EXPORT void quat2rot(const float Q[4], float R[9]); - -/** - * Calculates air density. - * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius - */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); - __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c deleted file mode 100644 index 6463e6489..000000000 --- a/src/modules/systemlib/geo/geo.c +++ /dev/null @@ -1,438 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file geo.c - * - * Geo / math functions to perform geodesic calculations - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include - - -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 -{ - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; - -} - -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); - - if (0 != c) - k_bar = c / sin(c); - - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; - -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); -} - -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); - double sin_c = sin(c); - double cos_c = cos(c); - - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); - - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); - - } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } - } - -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - -} - - -__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0d * M_PI; - double lon_now_rad = lon_now / 180.0d * M_PI; - double lat_next_rad = lat_next / 180.0d * M_PI; - double lon_next_rad = lon_next / 180.0d * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - - const double radius_earth = 6371000.0d; - return radius_earth * c; -} - -__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now * M_DEG_TO_RAD; - double lon_now_rad = lon_now * M_DEG_TO_RAD; - double lat_next_rad = lat_next * M_DEG_TO_RAD; - double lon_next_rad = lon_next * M_DEG_TO_RAD; - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - theta = _wrap_pi(theta); - - return theta; -} - -// Additional functions - @author Doug Weibel - -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) -{ -// This function returns the distance to the nearest point on the track line. Distance is positive if current -// position is right of the track and negative if left of the track as seen from a point on the track line -// headed towards the end point. - - float dist_to_end; - float bearing_end; - float bearing_track; - float bearing_diff; - - int return_value = ERROR; // Set error flag, cleared when valid result calculated. - crosstrack_error->past_end = false; - crosstrack_error->distance = 0.0f; - crosstrack_error->bearing = 0.0f; - - // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; - - bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); - bearing_diff = bearing_track - bearing_end; - bearing_diff = _wrap_pi(bearing_diff); - - // Return past_end = true if past end point of line - if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { - crosstrack_error->past_end = true; - return_value = OK; - return return_value; - } - - dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); - - if (sin(bearing_diff) >= 0) { - crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); - - } else { - crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); - } - - return_value = OK; - - return return_value; - -} - - -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) -{ - // This function returns the distance to the nearest point on the track arc. Distance is positive if current - // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and - // headed towards the end point. - - // Determine if the current position is inside or outside the sector between the line from the center - // to the arc start and the line from the center to the arc end - float bearing_sector_start; - float bearing_sector_end; - float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - bool in_sector; - - int return_value = ERROR; // Set error flag, cleared when valid result calculated. - crosstrack_error->past_end = false; - crosstrack_error->distance = 0.0f; - crosstrack_error->bearing = 0.0f; - - // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; - - - if (arc_sweep >= 0) { - bearing_sector_start = arc_start_bearing; - bearing_sector_end = arc_start_bearing + arc_sweep; - - if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; - - } else { - bearing_sector_end = arc_start_bearing; - bearing_sector_start = arc_start_bearing - arc_sweep; - - if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; - } - - in_sector = false; - - // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; - - // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; - - // If in the sector then calculate distance and bearing to closest point - if (in_sector) { - crosstrack_error->past_end = false; - float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - - if (dist_to_center <= radius) { - crosstrack_error->distance = radius - dist_to_center; - crosstrack_error->bearing = bearing_now + M_PI_F; - - } else { - crosstrack_error->distance = dist_to_center - radius; - crosstrack_error->bearing = bearing_now; - } - - // If out of the sector then calculate dist and bearing to start or end point - - } else { - - // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) - // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to - // calculate the position of the start and end points. We should not be doing this often - // as this function generally will not be called repeatedly when we are out of the sector. - - // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; - float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - - - if (dist_to_start < dist_to_end) { - crosstrack_error->distance = dist_to_start; - crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - - } else { - crosstrack_error->past_end = true; - crosstrack_error->distance = dist_to_end; - crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - } - - } - - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); - return_value = OK; - return return_value; -} - -__EXPORT float _wrap_pi(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { - return bearing; - } - - int c = 0; - - while (bearing > M_PI_F && c < 30) { - bearing -= M_TWOPI_F; - c++; - } - - c = 0; - - while (bearing <= -M_PI_F && c < 30) { - bearing += M_TWOPI_F; - c++; - } - - return bearing; -} - -__EXPORT float _wrap_2pi(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing)) { - return bearing; - } - - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; - } - - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; - } - - return bearing; -} - -__EXPORT float _wrap_180(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing)) { - return bearing; - } - - while (bearing > 180.0f) { - bearing = bearing - 360.0f; - } - - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - -__EXPORT float _wrap_360(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing)) { - return bearing; - } - - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; - } - - while (bearing < 0.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - - diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h deleted file mode 100644 index dadec51ec..000000000 --- a/src/modules/systemlib/geo/geo.h +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file geo.h - * - * Definition of geo / math functions to perform geodesic calculations - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * Additional functions - @author Doug Weibel - */ - -#pragma once - -__BEGIN_DECLS - -#include - -#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ -#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ -#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ -#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ - -/* compatibility aliases */ -#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH -#define GRAVITY_MSS CONSTANTS_ONE_G - -// XXX remove -struct crosstrack_error_s { - bool past_end; // Flag indicating we are past the end of the line/arc segment - float distance; // Distance in meters to closest point on line/arc - float bearing; // Bearing in radians to closest point on line/arc -} ; - -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT void map_projection_init(double lat_0, double lon_0); - -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); - -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); - -/** - * Returns the distance to the next waypoint in meters. - * - * @param lat_now current position in degrees (47.1234567°, not 471234567°) - * @param lon_now current position in degrees (8.1234567°, not 81234567°) - * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) - * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) - */ -__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); - -/** - * Returns the bearing to the next waypoint in radians. - * - * @param lat_now current position in degrees (47.1234567°, not 471234567°) - * @param lon_now current position in degrees (8.1234567°, not 81234567°) - * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) - * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) - */ -__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); - -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); - -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); - -__EXPORT float _wrap_180(float bearing); -__EXPORT float _wrap_360(float bearing); -__EXPORT float _wrap_pi(float bearing); -__EXPORT float _wrap_2pi(float bearing); - -__END_DECLS diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index cbf829122..94c744c03 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -45,7 +45,6 @@ SRCS = err.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ - geo/geo.c \ systemlib.c \ airspeed.c \ system_params.c \ -- cgit v1.2.3 From 3005c8aae0ed0c74d2d611712d9e7d659f4ed453 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Aug 2013 18:36:10 +0200 Subject: Added missing config files, compiling --- makefiles/config_px4fmu-v1_default.mk | 15 +++++++-------- makefiles/config_px4fmu-v2_default.mk | 17 +++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cca518552..a556d0b07 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -70,8 +70,6 @@ MODULES += modules/gpio_led # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3_comp -MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator @@ -80,9 +78,8 @@ MODULES += examples/flow_position_estimator # Vehicle Control # #MODULES += modules/segway # XXX needs state machine update -MODULES += modules/fixedwing_backside -MODULES += modules/fixedwing_att_control -MODULES += modules/fixedwing_pos_control +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control @@ -98,15 +95,17 @@ MODULES += modules/sdlog2 # MODULES += modules/systemlib MODULES += modules/systemlib/mixer -MODULES += modules/mathlib -MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB # # Libraries # -LIBRARIES += modules/mathlib/CMSIS +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +#MODULES += lib/ecl +MODULES += lib/geo # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 101b49712..20dbe717f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -66,16 +66,15 @@ MODULES += modules/mavlink_onboard # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/position_estimator_mc -MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/fixedwing_backside -MODULES += modules/fixedwing_att_control -MODULES += modules/fixedwing_pos_control +#MODULES += modules/fw_pos_control_l1 +#MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control @@ -89,15 +88,17 @@ MODULES += modules/sdlog2 # MODULES += modules/systemlib MODULES += modules/systemlib/mixer -MODULES += modules/mathlib -MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB # # Libraries # -LIBRARIES += modules/mathlib/CMSIS +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +#MODULES += lib/ecl +MODULES += lib/geo # # Demo apps -- cgit v1.2.3 From cfa9054aa4e6accae1fefaad1a13316301ce56fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 09:25:13 +0200 Subject: Moved to USART1 for the main console, starting a 2nd NSH instance on USB if needed, reworked start scripts to not fall over --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 14 ++++---- ROMFS/px4fmu_common/init.d/02_io_quad_x | 2 +- ROMFS/px4fmu_common/init.d/08_ardrone | 25 ++++----------- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 15 +++++---- ROMFS/px4fmu_common/init.d/10_io_f330 | 51 +++++++++++++++--------------- ROMFS/px4fmu_common/init.d/rcS | 47 ++++++++++++++++++++++++++- nuttx-configs/px4fmu-v1/nsh/defconfig | 8 ++--- 7 files changed, 97 insertions(+), 65 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 58a970eba..c1f3187f9 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -97,11 +97,9 @@ multirotor_att_control start # Start logging # sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi + +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index c63e92f6d..49483d14f 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -88,7 +88,7 @@ pwm -u 400 -m 0xff multirotor_att_control start # -# Start logging +# Start logging once armed # sdlog2 start -r 50 -a -b 14 diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae3..5bb1491e9 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -2,12 +2,6 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone echo "[init] doing PX4IOAR startup..." @@ -70,25 +64,14 @@ multirotor_att_control start ardrone_interface start -d /dev/ttyS1 # -# Start logging +# Start logging once armed # -sdlog start -s 10 +sdlog2 start -r 50 -a -b 14 # # Start GPS capture # gps start - -# -# Start system state -# -if blinkm start -then - echo "using BlinkM for state indication" - blinkm systemstate -else - echo "no BlinkM found, OK." -fi # # startup is done; we don't want the shell because we @@ -96,4 +79,8 @@ fi # echo "[init] startup done" +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6..a223ef7aa 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -2,12 +2,6 @@ # # Flight startup script for PX4FMU on PX4IOAR carrier board. # - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE ardrone echo "[init] doing PX4IOAR startup..." @@ -84,6 +78,11 @@ flow_speed_control start # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 + +# +# Start logging once armed +# +sdlog2 start -r 50 -a -b 14 # # startup is done; we don't want the shell because we @@ -91,4 +90,8 @@ ardrone_interface start -d /dev/ttyS1 # echo "[init] startup done" +# Try to get an USB console +nshterm /dev/ttyACM0 & + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 0e6d3f5d5..b2fc0c96f 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -2,10 +2,6 @@ # # Flight startup script for PX4FMU+PX4IO on an F330 quad. # - -# disable USB and autostart -set USB no -set MODE custom # # Start the ORB (first app to start) @@ -60,33 +56,36 @@ commander start # if px4io start then + # + # This sets a PWM right after startup (regardless of safety button) + # + px4io idle 900 900 900 900 + pwm -u 400 -m 0xff -else - # SOS - tone_alarm 6 -fi - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery -# -# This sets a PWM right after startup (regardless of safety button) -# -px4io idle 900 900 900 900 + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery -# -# The values are for spinning motors when armed using DJI ESCs -# -px4io min 1200 1200 1200 1200 -# -# Upper limits could be higher, this is on the safe side -# -px4io max 1800 1800 1800 1800 -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + # + # The values are for spinning motors when armed using DJI ESCs + # + px4io min 1200 1200 1200 1200 + + # + # Upper limits could be higher, this is on the safe side + # + px4io max 1800 1800 1800 1800 + + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +else + # SOS + tone_alarm 6 +fi # # Start the sensors (depends on orb, px4io) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 650224cf1..8674c3c04 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -61,7 +61,13 @@ then # # Start terminal # -sercon +if sercon +then + echo "USB connected" +else + # second attempt + sercon & +fi # # Start the ORB (first app to start) @@ -128,45 +134,84 @@ fi if param compare SYS_AUTOSTART 1 then sh /etc/init.d/01_fmu_quad_x + set MODE custom fi if param compare SYS_AUTOSTART 2 then sh /etc/init.d/02_io_quad_x + set MODE custom fi if param compare SYS_AUTOSTART 8 then sh /etc/init.d/08_ardrone + set MODE custom fi if param compare SYS_AUTOSTART 9 then sh /etc/init.d/09_ardrone_flow + set MODE custom fi if param compare SYS_AUTOSTART 10 then sh /etc/init.d/10_io_f330 + set MODE custom fi if param compare SYS_AUTOSTART 15 then sh /etc/init.d/15_io_tbs + set MODE custom fi if param compare SYS_AUTOSTART 30 then sh /etc/init.d/30_io_camflyer + set MODE custom fi if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom + set MODE custom fi # Try to get an USB console nshterm /dev/ttyACM0 & +# If none of the autostart scripts triggered, get a minimal setup +if [ $MODE == autostart ] +then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + + # Start commander + commander start + + # Start px4io if present + if px4io start + then + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi + fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + +fi + # End of autostart fi diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c6..412a29fbe 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -368,7 +368,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=0 @@ -476,11 +476,11 @@ CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 -# CONFIG_USART1_SERIAL_CONSOLE is not set +CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_UART5_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -550,7 +550,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 -- cgit v1.2.3 From 11257cbade5d89d3d2de8101daec11d32f7f74ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 10:13:47 +0200 Subject: Fixed commandline handling --- src/drivers/px4io/px4io.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c00816a12..9fc07392c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1802,13 +1802,15 @@ start(int argc, char *argv[]) } /* disable RC handling on request */ - if (argc > 0 && !strcmp(argv[0], "norc")) { + if (argc > 1) { + if (!strcmp(argv[1], "norc")) { - if(g_dev->disable_rc_handling()) - warnx("Failed disabling RC handling"); + if(g_dev->disable_rc_handling()) + warnx("Failed disabling RC handling"); - } else { - warnx("unknown argument: %s", argv[0]); + } else { + warnx("unknown argument: %s", argv[1]); + } } int dsm_vcc_ctl; -- cgit v1.2.3 From 85eafa323aec397e4ed5c394f25d48ce6d878f9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Aug 2013 10:43:19 +0200 Subject: Fix to RC param updates on IO --- ROMFS/px4fmu_common/init.d/rcS | 1 + src/modules/px4iofirmware/registers.c | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8674c3c04..6b624b278 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -187,6 +187,7 @@ if [ $MODE == autostart ] then # Telemetry port is on both FMU boards ttyS1 mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 # Start commander commander start diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 655a0c7a8..9c95fd1c5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -499,8 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /** + * do not allow a RC config change while outputs armed + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } -- cgit v1.2.3 From ab5ec0da0b4afcb3f68eaf70efebe691513f74ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:19 +0200 Subject: Changed the default PID gains for the F330 --- ROMFS/px4fmu_common/init.d/10_io_f330 | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index b2fc0c96f..50842764a 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -16,19 +16,19 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.005 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.1 + param set MC_ATTRATE_P 0.12 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 4.5 + param set MC_ATT_P 7.0 param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.3 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.1 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 param save /fs/microsd/params fi -- cgit v1.2.3 From ca96140b217971d527e2306922a87a1592e6f90d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:53:46 +0200 Subject: Allow the tone alarms to be interrupted --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index b06920a76..ad21f7143 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -879,14 +879,9 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - /* don't interrupt alarms unless they are repeated */ - if (_tune != nullptr && !_repeat) { - /* abort and let the current tune finish */ - result = -EBUSY; - } else if (_repeat && _default_tune_number == arg) { - /* requested repeating tune already playing */ - } else { - // play the selected tune + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ _default_tune_number = arg; start_tune(_default_tunes[arg - 1]); } -- cgit v1.2.3 From 6c3da5aeddf929f5a4f19f6bd1b75c911c2a414c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:55:33 +0200 Subject: Reset yaw position when disarmed in multirotor controller --- .../multirotor_att_control_main.c | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef364..2d16d4921 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -144,10 +144,6 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; - bool control_yaw_position = true; bool reset_yaw_sp = true; @@ -232,12 +228,6 @@ mc_thread_main(int argc, char *argv[]) } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - /* initialize to current yaw if switching to manual or att control */ - if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { - att_sp.yaw_body = att.yaw; - } static bool rc_loss_first_time = true; @@ -283,12 +273,12 @@ mc_thread_main(int argc, char *argv[]) /* control yaw in all manual / assisted modes */ /* set yaw if arming or switching to attitude stabilized mode */ - if (!flag_control_attitude_enabled) { + if (!control_mode.flag_armed) { reset_yaw_sp = true; } /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { // TODO use landed status instead of throttle rates_sp.yaw = manual.yaw; control_yaw_position = false; reset_yaw_sp = true; @@ -377,10 +367,6 @@ mc_thread_main(int argc, char *argv[]) actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; - flag_control_manual_enabled = control_mode.flag_control_manual_enabled; - perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ } /* end of poll return value check */ -- cgit v1.2.3 From 5f1004117f8086c4bba5b4031f3aebd73411682c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:57:17 +0200 Subject: Restore proper feedback (mavlink and tone) for calibration commands, etc --- .../commander/accelerometer_calibration.cpp | 6 +- src/modules/commander/accelerometer_calibration.h | 2 +- src/modules/commander/airspeed_calibration.cpp | 18 +- src/modules/commander/airspeed_calibration.h | 4 +- src/modules/commander/baro_calibration.cpp | 10 +- src/modules/commander/baro_calibration.h | 4 +- src/modules/commander/commander.cpp | 206 +++++++++++---------- src/modules/commander/gyro_calibration.cpp | 31 ++-- src/modules/commander/gyro_calibration.h | 4 +- src/modules/commander/mag_calibration.cpp | 17 +- src/modules/commander/mag_calibration.h | 4 +- src/modules/commander/rc_calibration.cpp | 13 +- src/modules/commander/rc_calibration.h | 4 +- 13 files changed, 183 insertions(+), 140 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ddd2e23d9..c2b2e9258 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,7 +133,7 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int mavlink_fd) { +int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); @@ -176,11 +176,11 @@ void do_accel_calibration(int mavlink_fd) { } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_positive(); + return OK; } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_negative(); + return ERROR; } /* exit accel calibration mode */ diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 3275d9461..1cf9c0977 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,6 +45,6 @@ #include -void do_accel_calibration(int mavlink_fd); +int do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index df08292e3..e414e5f70 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,7 +49,13 @@ #include #include -void do_airspeed_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); @@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; + return ERROR; } } @@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + return ERROR; } /* auto-save to EEPROM */ @@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } //char buf[50]; @@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_positive(); + return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + return ERROR; } - - close(diff_pres_sub); } diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 92f5651ec..71c701fc2 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -41,6 +41,6 @@ #include -void do_airspeed_calibration(int mavlink_fd); +int do_airspeed_calibration(int mavlink_fd); -#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index d7515b3d9..3123c4087 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -47,8 +47,14 @@ #include #include -void do_baro_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_baro_calibration(int mavlink_fd) { // TODO implement this - return; + return ERROR; } diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index ac0f4be36..bc42bc6cf 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -41,6 +41,6 @@ #include -void do_baro_calibration(int mavlink_fd); +int do_baro_calibration(int mavlink_fd); -#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17db0f9c8..66b9272de 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + /* try again later */ + usleep(20000); + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); + } } /* Main state machine */ @@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v return res; } +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + default: + break; + } +} + void *commander_low_prio_loop(void *arg) { /* Set thread name */ @@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg) cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) continue; - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle low-priority commands here */ switch (cmd.command) { case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { - - if (((int)(cmd.param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd.param1)) == 3) { - /* reboot to bootloader */ - systemreset(true); - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot to bootloader */ + systemreset(true); } else { - result = VEHICLE_CMD_RESULT_DENIED; + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + break; - /* try to go to INIT/PREFLIGHT arming state */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - result = VEHICLE_CMD_RESULT_DENIED; - break; - } + int calib_ret = ERROR; - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - do_gyro_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* try to go to INIT/PREFLIGHT arming state */ - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - do_mag_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - do_accel_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - do_airspeed_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } - - break; } - - default: break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); - - } else { - tune_negative(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - } + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; } /* send any requested ACKs */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index f1afb0107..9cd2f3a19 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -50,8 +50,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_gyro_calibration(int mavlink_fd) +int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); @@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; + return ERROR; } } @@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); - // XXX negative tune - return; + return ERROR; } mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); + tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - return; + return ERROR; } @@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd) && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); - return; + return OK; } /* wait blocking for new data */ @@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd) // operating near the 1e6/1e8 max sane resolution of float. gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); +// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); // } else if (poll_ret == 0) { // /* any poll failure for 1s is a reason to abort */ @@ -232,8 +235,8 @@ void do_gyro_calibration(int mavlink_fd) float gyro_scale = baseline_integral / gyro_integral; float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; - warnx("gyro scale: yaw (z): %6.4f", gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); /* third beep by cal end routine */ - + return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + return ERROR; } - - close(sub_sensor_combined); } diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h index cd262507d..59c32a15e 100644 --- a/src/modules/commander/gyro_calibration.h +++ b/src/modules/commander/gyro_calibration.h @@ -41,6 +41,6 @@ #include -void do_gyro_calibration(int mavlink_fd); +int do_gyro_calibration(int mavlink_fd); -#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9a25103f8..9263c6a15 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -53,8 +53,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_mag_calibration(int mavlink_fd) +int do_mag_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); @@ -113,7 +118,7 @@ void do_mag_calibration(int mavlink_fd) warnx("mag cal failed: out of memory"); mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); warnx("x:%p y:%p z:%p\n", x, y, z); - return; + return ERROR; } while (hrt_absolute_time() < calibration_deadline && @@ -252,6 +257,7 @@ void do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", @@ -269,12 +275,11 @@ void do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_positive(); + return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + return ERROR; } - - close(sub_mag); -} \ No newline at end of file +} diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index fd2085f14..a101cd7b1 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -41,6 +41,6 @@ #include -void do_mag_calibration(int mavlink_fd); +int do_mag_calibration(int mavlink_fd); -#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file +#endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0de411713..fe87a3323 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -46,8 +46,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_rc_calibration(int mavlink_fd) +int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); @@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + return ERROR; } - tune_positive(); - mavlink_log_info(mavlink_fd, "trim calibration done"); -} \ No newline at end of file + return OK; +} diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 6505c7364..9aa6faafa 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include -void do_rc_calibration(int mavlink_fd); +int do_rc_calibration(int mavlink_fd); -#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file +#endif /* RC_CALIBRATION_H_ */ -- cgit v1.2.3 From b5bb20995be8c0f55bed4f2f2bd6cee9efdcf03e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:31:59 +0200 Subject: multirotor_att_control: yaw setpoint reset fix --- .../multirotor_att_control_main.c | 103 +++++++++++---------- 1 file changed, 53 insertions(+), 50 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c057ef364..2d46bf438 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -81,6 +81,8 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static const float min_takeoff_throttle = 0.3f; +static const float yaw_deadzone = 0.01f; static int mc_thread_main(int argc, char *argv[]) @@ -147,14 +149,14 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool control_yaw_position = true; bool reset_yaw_sp = true; + bool failsafe_first_time = true; /* prepare the handle for the failsafe throttle */ param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - + param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +178,7 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - // XXX no params here yet + param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -208,6 +210,9 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + /* set flag to safe value */ + control_yaw_position = true; + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ @@ -225,47 +230,40 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } else if (control_mode.flag_control_manual_enabled) { - /* direct manual input */ + /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* initialize to current yaw if switching to manual or att control */ - if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { + failsafe_first_time = false; + if (!control_mode.flag_control_velocity_enabled) { - /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ + /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; + } - if (!control_mode.flag_control_climb_rate_enabled) { - /* Don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } + if (!control_mode.flag_control_climb_rate_enabled) { + /* don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + att_sp.thrust = failsafe_throttle; + + } else { + att_sp.thrust = 0.0f; } } @@ -273,46 +271,48 @@ mc_thread_main(int argc, char *argv[]) * since if the pilot regains RC control, he will be lost regarding * the current orientation. */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; + if (failsafe_first_time) { + reset_yaw_sp = true; + } } else { - rc_loss_first_time = true; + failsafe_first_time = true; /* control yaw in all manual / assisted modes */ /* set yaw if arming or switching to attitude stabilized mode */ - if (!flag_control_attitude_enabled) { + if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) { reset_yaw_sp = true; } /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle - rates_sp.yaw = manual.yaw; + // TODO review yaw restpoint reset + if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) { + /* control yaw rate */ control_yaw_position = false; - reset_yaw_sp = true; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log } else { - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } control_yaw_position = true; } if (!control_mode.flag_control_velocity_enabled) { - /* don't update attitude setpoint in position control mode */ + /* update attitude setpoint if not in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; if (!control_mode.flag_control_climb_rate_enabled) { - /* don't set throttle in altitude hold modes */ + /* pass throttle directly if not in altitude control mode */ att_sp.thrust = manual.throttle; } } - att_sp.timestamp = hrt_absolute_time(); + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; } if (motor_test_mode) { @@ -321,10 +321,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); } - /* STEP 2: publish the controller output */ + att_sp.timestamp = hrt_absolute_time(); + + /* publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -367,6 +368,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -374,6 +376,7 @@ mc_thread_main(int argc, char *argv[]) actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); -- cgit v1.2.3 From bb91484b2648800923a135be28924119a1382ba6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:34:59 +0200 Subject: Default flight mode switches parameters changed. --- src/modules/sensors/sensor_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2bd869263..8c2beb456 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,8 +167,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); -- cgit v1.2.3 From 41fac46ff08787d9b2e4d902045d65c205389abd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:05:30 +0200 Subject: mavlink VFR_HUD message fixed, minor fixes and cleanup --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mavlink/orb_listener.c | 10 +++++----- .../position_estimator_inav_main.c | 2 +- src/modules/uORB/topics/vehicle_global_position.h | 18 +++++++++--------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04e6dd2cb..4580c57bc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -825,9 +825,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = local_position.landed; status_changed = true; if (status.condition_landed) { - mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); } else { - mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d..97cf571e5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -637,12 +637,12 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3466841c4..d35755b4f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -610,7 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f036c7223..822c323cf 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,17 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees LOGME */ - float alt; /**< Altitude in meters LOGME */ - float relative_alt; /**< Altitude above home position in meters, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + float alt; /**< Altitude in meters */ + float relative_alt; /**< Altitude above home position in meters, */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float hdg; /**< Compass heading in radians -PI..+PI. */ }; -- cgit v1.2.3 From 330908225e5fcb1731df20e740dbfe403a7b30b9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:23:42 +0200 Subject: sdlog2: free buffer on exit --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7f8648d95..2b21bb5a5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1182,6 +1182,8 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); + free(lb.data); + warnx("exiting."); thread_running = false; -- cgit v1.2.3 From ca9aabf48bbf4ab050a13f12e8152490e2b1dbde Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:29:06 -0700 Subject: add placeholder autoconfig file for X550 --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 107 +++++++++++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 new file mode 100644 index 000000000..41593fa6c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi -- cgit v1.2.3 From 64f402ce839f619572bb45a7b1a3126a2cd96d88 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:29:06 -0700 Subject: add placeholder autoconfig file for X550 --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 107 +++++++++++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 new file mode 100644 index 000000000..41593fa6c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi -- cgit v1.2.3 From 201bda457941fa55961eea4f13ec9fc9c24c0c61 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Thu, 22 Aug 2013 17:36:10 -0700 Subject: start position estimator and position control --- ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 index 41593fa6c..a7ffffaee 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 @@ -82,6 +82,10 @@ gps start # Start the attitude estimator # attitude_estimator_ekf start +# +# Start the position estimator +# +position_estimator_inav start echo "[init] starting PWM output" fmu mode_pwm @@ -93,6 +97,11 @@ pwm -u 400 -m 0xff # multirotor_att_control start +# +# Start position control +# +multirotor_pos_control start + # # Start logging # -- cgit v1.2.3 From f70a4b3b7045a24baf70e642e9a354a13e5fa7d1 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 22 Aug 2013 23:47:55 -0700 Subject: Add support for adding extra files to the ROMFS from the config. If there is an IO firmware image already built when we build the corresponding FMU ROMFS, copy it into the ROMFS. Note - due to there being no fixed build ordering, to be certain that you have the most current IO firmware, you must build the IO firmware explicitly first. --- Makefile | 26 +++++++++++++------------- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 4 +++- makefiles/firmware.mk | 22 +++++++++++++++++++--- makefiles/setup.mk | 1 + 5 files changed, 37 insertions(+), 17 deletions(-) diff --git a/Makefile b/Makefile index f7cab05c8..c34945eca 100644 --- a/Makefile +++ b/Makefile @@ -114,8 +114,8 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% - $(Q) mkdir -p $(work_dir) - $(Q) make -r -C $(work_dir) \ + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -147,12 +147,12 @@ $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export - $(Q) mkdir -p $(dir $@) + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -168,11 +168,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else @@ -191,7 +191,7 @@ $(NUTTX_SRC): # Testing targets # testbuild: - $(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8) + $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) # # Cleanup targets. 'clean' should remove all built products and force @@ -206,8 +206,8 @@ clean: .PHONY: distclean distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && find . -maxdepth 1 -type l -delete) + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) # # Print some help text @@ -229,9 +229,9 @@ help: @$(ECHO) " A limited set of configs can be built with CONFIGS=" @$(ECHO) "" @for config in $(CONFIGS); do \ - echo " $$config"; \ - echo " Build just the $$config firmware configuration."; \ - echo ""; \ + $(ECHO) " $$config"; \ + $(ECHO) " Build just the $$config firmware configuration."; \ + $(ECHO) ""; \ done @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 8f2ade8dc..0e4617f1d 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -6,6 +6,7 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin # # Board support modules diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 101b49712..61df64a78 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -3,9 +3,11 @@ # # -# Use the configuration's ROMFS. +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 8a027a0a8..b3e50501c 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -322,7 +322,7 @@ endif # a root from several templates. That would be a nice feature. # -# Add dependencies on anything in the ROMFS root +# Add dependencies on anything in the ROMFS root directory ROMFS_FILES += $(wildcard \ $(ROMFS_ROOT)/* \ $(ROMFS_ROOT)/*/* \ @@ -334,7 +334,14 @@ ifeq ($(ROMFS_FILES),) $(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) endif ROMFS_DEPS += $(ROMFS_FILES) + +# Extra files that may be copied into the ROMFS /extras directory +# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional +ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) +ROMFS_DEPS += $(ROMFS_EXTRA_FILES) + ROMFS_IMG = romfs.img +ROMFS_SCRATCH = romfs_scratch ROMFS_CSRC = $(ROMFS_IMG:.img=.c) ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) @@ -345,9 +352,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) $(call BIN_TO_OBJ,$<,$@,romfs_img) # Generate the ROMFS image from the root -$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS) +$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) @$(ECHO) "ROMFS: $@" - $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" + +# Construct the ROMFS scratch root from the canonical root +$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) + $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +ifneq ($(ROMFS_EXTRA_FILES),) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras + $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras +endif EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5c..51f830b6e 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -71,6 +71,7 @@ export RMDIR = rm -rf export GENROMFS = genromfs export TOUCH = touch export MKDIR = mkdir +export FIND = find export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python -- cgit v1.2.3 From 54711bbcfe52fe33c213b9e15a1da9ad978d3535 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 23 Aug 2013 00:23:32 -0700 Subject: In order to save people from themselves, force a given FMU version to depend on the corresponding _default IO version. This avoids the risk of building a new FMU ROMFS with an old IO firmware, at the cost of the sanity of anyone reading this. --- Makefile | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Makefile b/Makefile index c34945eca..cbf0da6c9 100644 --- a/Makefile +++ b/Makefile @@ -121,6 +121,19 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) +# +# Make FMU firmwares depend on pre-packaged IO binaries. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + # # Build the NuttX export archives. # -- cgit v1.2.3 From d1fd1bbbf7c7fd6bc76137257d6ecb91e8b6f3d4 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 23 Aug 2013 13:27:16 -0700 Subject: Fix timestamp on rates_sp --- src/modules/multirotor_att_control/multirotor_attitude_control.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 12d16f7c7..c78232f11 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -247,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } rates_sp->thrust = att_sp->thrust; + //need to update the timestamp now that we've touched rates_sp + rates_sp->timestamp = hrt_absolute_time(); motor_skip_counter++; } -- cgit v1.2.3 From 5e9b508ea0ec799ab6f8723d114c999beffc347e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 23 Aug 2013 23:03:59 +0200 Subject: Indicate AUTO submodes in mavlink custom_mode. --- src/modules/commander/commander.cpp | 68 +++++++++++++++++--------------- src/modules/commander/px4_custom_mode.h | 29 +++++++++++--- src/modules/mavlink/mavlink.c | 24 +++++++++-- src/modules/mavlink/mavlink_receiver.cpp | 5 ++- 4 files changed, 84 insertions(+), 42 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4580c57bc..8bde6b7a9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -274,7 +274,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - uint32_t custom_mode = (uint32_t) cmd->param2; + union px4_custom_mode custom_mode; + custom_mode.data_float = cmd->param2; // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); @@ -307,19 +308,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } @@ -1544,43 +1545,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; - - } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } else { + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } - } - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } } + } else { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); } - break; default: diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 4d4859a5c..b60a7e0b9 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,11 +8,30 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +enum PX4_CUSTOM_SUB_MODE_AUTO { + PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION, + PX4_CUSTOM_SUB_MODE_AUTO_RTL, + PX4_CUSTOM_SUB_MODE_AUTO_LAND, +}; + +union px4_custom_mode { + struct { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + }; + uint32_t data; + float data_float; }; #endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120..93ec36d05 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u /* main state */ *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; } else if (v_status.main_state == MAIN_STATE_EASY) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } } + *mavlink_custom_mode = custom_mode.data; /** * Set mavlink state diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..86fa73656 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -67,6 +67,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -188,9 +189,11 @@ handle_message(mavlink_message_t *msg) mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_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.param2 = custom_mode.data_float; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; -- cgit v1.2.3 From 8579d0b7c9f77c84fa9afa87f7ab2f353443a242 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:31:01 +0200 Subject: Allow disarm by RC in assisted modes if landed and in AUTO_READY state. --- src/modules/commander/commander.cpp | 55 ++++++++++++++++++------------------- 1 file changed, 26 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fb..f5a9d4088 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1017,46 +1017,42 @@ int commander_thread_main(int argc, char *argv[]) /* arm/disarm by RC */ res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - stick_on_counter = 0; + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } + } else { + stick_off_counter = 0; } - /* check if left stick is in lower right position and we're in manual mode -> arm */ + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL) { - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - stick_on_counter = 0; - - } else { - stick_on_counter++; - } - - stick_off_counter = 0; + status.main_state == MAIN_STATE_MANUAL && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { - stick_on_counter = 0; + stick_on_counter++; } + + } else { + stick_on_counter = 0; } if (res == TRANSITION_CHANGED) { @@ -1702,7 +1698,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF) continue; /* only handle low-priority commands here */ -- cgit v1.2.3 From 41dfdfb1a4b974b5d32788852768513d0dac7a67 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:32:46 +0200 Subject: Use common rc.multirotor script (now only in 01_fmu_quad_x). --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 33 ++++----------------- ROMFS/px4fmu_common/init.d/rc.multirotor | 49 ++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 28 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index c1f3187f9..8223b3ea5 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -62,44 +62,21 @@ param set MAV_TYPE 2 # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start # -# Start GPS interface (depends on orb) +# Start common for all multirotors apps # -gps start - +sh /etc/init.d/rc.multirotor + # -# Start the attitude estimator +# Start PWM output # -attitude_estimator_ekf start - -echo "[init] starting PWM output" fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm -u 400 -m 0xff -# -# Start attitude control -# -multirotor_att_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - # Try to get an USB console nshterm /dev/ttyACM0 & # Exit, because /dev/ttyS0 is needed for MAVLink -exit +#exit diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor new file mode 100644 index 000000000..81184f363 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -0,0 +1,49 @@ +#!nsh +# +# Standard everything needed for multirotors except mixer, output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +# +# Start logging +# +if [ $BOARD == fmuv1 ] +then + sdlog2 start -r 50 -a -b 16 +else + sdlog2 start -r 200 -a -b 16 +fi -- cgit v1.2.3 From e119bbb0f1161c71b8c2dcbbbc150d40b356c4b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 16:33:14 +0200 Subject: A lot more on calibration and RC checks. Needs more testing, but no known issues --- ROMFS/px4fmu_common/init.d/rcS | 6 + .../commander/accelerometer_calibration.cpp | 18 ++- src/modules/commander/commander.cpp | 6 + src/modules/commander/mag_calibration.cpp | 10 +- src/modules/sensors/sensor_params.c | 10 +- src/modules/sensors/sensors.cpp | 37 +----- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/rc_check.c | 148 +++++++++++++++++++++ src/modules/systemlib/rc_check.h | 52 ++++++++ src/systemcmds/preflight_check/preflight_check.c | 97 +------------- 10 files changed, 246 insertions(+), 141 deletions(-) create mode 100644 src/modules/systemlib/rc_check.c create mode 100644 src/modules/systemlib/rc_check.h diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b278..30edf679a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,6 +182,12 @@ fi # Try to get an USB console nshterm /dev/ttyACM0 & +# Start any custom extensions that might be missing +if [ -f /fs/microsd/etc/rc.local ] +then + sh /fs/microsd/etc/rc.local +fi + # If none of the autostart scripts triggered, get a minimal setup if [ $MODE == autostart ] then diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c2b2e9258..82df7c37d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; + char str[60]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; + } else { + done_count++; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; mavlink_log_info(mavlink_fd, str); @@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + // sprintf(str, + mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fb..e3d314881 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; + bool rc_calibration_ok = (OK == rc_calibration_check()); + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); status_changed = true; + + /* Re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9263c6a15..42f0190c7 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd) close(fd); + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + /* calibrate offsets */ // uint64_t calibration_start = hrt_absolute_time(); @@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd) warnx("Setting Z mag scale failed!\n"); } + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + /* auto-save to EEPROM */ int save_ret = param_save_default(); @@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; /* third beep by cal end routine */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b45317dbe..fd2a6318e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ded39c465..e857b1c2f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -305,8 +305,6 @@ private: int board_rotation; int external_mag_rotation; - int rc_type; - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; @@ -342,9 +340,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -566,8 +561,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -692,11 +685,6 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -738,26 +726,11 @@ Sensors::parameters_update() // warnx("Failed getting offboard control mode sw chan index"); // } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); - } - - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); - } - - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } - + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 94c744c03..843cda722 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ systemlib.c \ airspeed.c \ system_params.c \ - mavlink_log.c + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 000000000..9d47d8000 --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * 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 rc_check.c + * + * RC calibration check + */ + +#include + +#include +#include + +#include +#include +#include +#include + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 000000000..e2238d151 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85f..4c19dcffb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include #include +#include __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + bool rc_ok = (OK == rc_calibration_check()); /* require RC ok to keep system_ok */ system_ok &= rc_ok; -- cgit v1.2.3 From 557d3f22de25a392995f2803363f32fdbc6ce843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 19:27:11 +0200 Subject: Startup scripts major cleanup --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 52 ++--- ROMFS/px4fmu_common/init.d/02_io_quad_x | 88 ++------ ROMFS/px4fmu_common/init.d/08_ardrone | 73 ++----- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 67 +++---- ROMFS/px4fmu_common/init.d/10_io_f330 | 80 ++------ ROMFS/px4fmu_common/init.d/15_io_tbs | 91 +-------- ROMFS/px4fmu_common/init.d/30_io_camflyer | 64 +----- ROMFS/px4fmu_common/init.d/31_io_phantom | 93 ++------- ROMFS/px4fmu_common/init.d/40_io_segway | 82 +------- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 64 ++++++ ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 | 116 ----------- ROMFS/px4fmu_common/init.d/rc.io | 23 +++ ROMFS/px4fmu_common/init.d/rc.logging | 7 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 10 - ROMFS/px4fmu_common/init.d/rcS | 287 ++++++++++++++------------- 15 files changed, 366 insertions(+), 831 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 create mode 100644 ROMFS/px4fmu_common/init.d/rc.io diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index 8223b3ea5..f57e4bd68 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -1,28 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" # # Load default params for this platform @@ -52,11 +30,10 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + # # Start MAVLink # @@ -64,19 +41,24 @@ mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 # -# Start common for all multirotors apps +# Start PWM output # -sh /etc/init.d/rc.multirotor +fmu mode_pwm # -# Start PWM output +# Load mixer # -fmu mode_pwm mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# pwm -u 400 -m 0xff - -# Try to get an USB console -nshterm /dev/ttyACM0 & +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink -#exit +exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x index 49483d14f..a37c26ad1 100644 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" # # Load default params for this platform @@ -28,74 +8,40 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 # -# Start MAVLink (depends on orb) +# Start MAVLink # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start - -# -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery # -# Disable px4io topic limiting -# -px4io limit 200 - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) +# Start and configure PX4IO interface # -attitude_estimator_ekf start - +sh /etc/init.d/rc.io + # -# Load mixer and start controllers (depends on px4io) +# Load mixer # mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff -multirotor_att_control start - + # -# Start logging once armed +# Set PWM output frequency # -sdlog2 start -r 50 -a -b 14 - +pwm -u 400 -m 0xff + # -# Start system state +# Start common for all multirotors apps # -if blinkm start -then - blinkm systemstate -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index 5bb1491e9..eb9f82f77 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -1,86 +1,45 @@ #!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start + +echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" # -# Load microSD params +# Load default params for this platform # -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - # # Start MAVLink # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - + # -# Fire up the multi rotor attitude controller +# Configure PX4FMU for operation with PX4IOAR # -multirotor_att_control start +fmu mode_gpio_serial # # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 - -# -# Start GPS capture -# -gps start # -# startup is done; we don't want the shell because we -# use the same UART for telemetry +# Start common for all multirotors apps # -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - +sh /etc/init.d/rc.multirotor + # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index a223ef7aa..44fbb79b7 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -1,49 +1,47 @@ #!nsh + +echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" + # -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params +# Load default params for this platform # -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +# +# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 + # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 # # Start the sensors. # sh /etc/init.d/rc.sensors -# -# Start MAVLink and MAVLink Onboard (Flow Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - # # Start the commander. # @@ -73,25 +71,6 @@ flow_position_control start # Fire up the flow speed controller # flow_speed_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start logging once armed -# -sdlog2 start -r 50 -a -b 14 -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - -# Try to get an USB console -nshterm /dev/ttyACM0 & - # Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 50842764a..7b6509bf8 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,12 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on an F330 quad. -# -# -# Start the ORB (first app to start) -# -uorb start +echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" # # Load default params for this platform @@ -35,8 +29,7 @@ fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -47,71 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) +# Set PWM values for DJI ESCs # -if px4io start -then - # - # This sets a PWM right after startup (regardless of safety button) - # - px4io idle 900 900 900 900 - - pwm -u 400 -m 0xff - - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - - - - # - # The values are for spinning motors when armed using DJI ESCs - # - px4io min 1200 1200 1200 1200 - - # - # Upper limits could be higher, this is on the safe side - # - px4io max 1800 1800 1800 1800 +px4io idle 900 900 900 900 +px4io min 1200 1200 1200 1200 +px4io max 1800 1800 1800 1800 - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -else - # SOS - tone_alarm 6 -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - # -# Start GPS interface (depends on orb) +# Load mixer # -gps start - +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + # -# Start the attitude estimator (depends on orb) +# Set PWM output frequency # -attitude_estimator_ekf start - -multirotor_att_control start +pwm -u 400 -m 0xff # -# Disable px4io topic limiting and start logging +# Start common for all multirotors apps # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index 237bb4267..b4f063e52 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -1,27 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO on a Team Blacksheep Discovery quad -# with stock DJI ESCs, motors and props. -# - -# disable USB and autostart -set USB no -set MODE custom -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi +echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" # # Load default params for this platform @@ -47,11 +26,10 @@ then param save /fs/microsd/params fi - + # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 @@ -62,77 +40,28 @@ mavlink start usleep 5000 # -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) -# -px4io start -pwm -u 400 -m 0xff - +# Start and configure PX4IO interface # -# Allow PX4IO to recover from midair restarts. -# This is very unlikely, but quite safe and robust. -px4io recovery +sh /etc/init.d/rc.io # -# This sets a PWM right after startup (regardless of safety button) +# Set PWM values for DJI ESCs # px4io idle 900 900 900 900 - -# -# The values are for spinning motors when armed using DJI ESCs -# px4io min 1180 1180 1180 1180 - -# -# Upper limits could be higher, this is on the safe side -# px4io max 1800 1800 1800 1800 # # Load the mixer for a quad with wide arms # mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start # -# Start the controllers (depends on orb) +# Set PWM output frequency # -multirotor_att_control start +pwm -u 400 -m 0xff # -# Disable px4io topic limiting and start logging +# Start common for all multirotors apps # -if [ $BOARD == fmuv1 ] -then - px4io limit 200 - sdlog2 start -r 50 -a -b 16 - if blinkm start - then - blinkm systemstate - fi -else - px4io limit 400 - sdlog2 start -r 200 -a -b 16 - if rgbled start - then - #rgbled systemstate - fi -fi +sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 5090b98a4..c9d5d6632 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -28,39 +8,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - # # Start MAVLink (depends on orb) # @@ -106,16 +65,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 5090b98a4..0deffe3f1 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,26 +1,6 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi + +echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" # # Load default params for this platform @@ -28,76 +8,50 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) + # -px4io start - +# Start and configure PX4IO interface # -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +sh /etc/init.d/rc.io # # Set actuator limit to 100 Hz update (50 Hz PWM) px4io limit 100 # -# Start the sensors (depends on orb, px4io) +# Start the commander +# +commander start + +# +# Start the sensors # sh /etc/init.d/rc.sensors # -# Start GPS interface (depends on orb) +# Start GPS interface # gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude estimator # kalman_demo start @@ -106,16 +60,3 @@ kalman_demo start # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix control_demo start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 5742d685a..2890f43be 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -1,26 +1,4 @@ #!nsh -# -# Flight startup script for PX4FMU+PX4IO -# - -# disable USB and autostart -set USB no -set MODE custom - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi # # Load default params for this platform @@ -28,39 +6,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 param save /fs/microsd/params fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 10 = ground rover # param set MAV_TYPE 10 - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log - echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" - fi - fi -fi - + # # Start MAVLink (depends on orb) # @@ -68,25 +25,15 @@ mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) -# -px4io start - +# Start the commander (depends on orb, mavlink) # -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +commander start -# -# Disable px4io topic limiting -# -px4io limit 200 - # # Start the sensors (depends on orb, px4io) # @@ -107,16 +54,3 @@ attitude_estimator_ekf start # md25 start 3 0x58 segway start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 new file mode 100644 index 000000000..2c8218013 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -0,0 +1,64 @@ +#!nsh + +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start PWM output +# +fmu mode_pwm + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +# Exit, because /dev/ttyS0 is needed for MAVLink +exit diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 deleted file mode 100644 index a7ffffaee..000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_quad_x550 +++ /dev/null @@ -1,116 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# disable USB and autostart -set USB no -set MODE custom - -echo "[init] doing PX4FMU Quad startup 666_fmu_quad_X550..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start -# -# Start the position estimator -# -position_estimator_inav start - -echo "[init] starting PWM output" -fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -pwm -u 400 -m 0xff - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -# -# Start logging -# -sdlog2 start -r 50 -a -b 14 - -# -# Start system state -# -if blinkm start -then - blinkm systemstate -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io new file mode 100644 index 000000000..85f00e582 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -0,0 +1,23 @@ +# +# Start PX4IO interface (depends on orb, commander) +# +if px4io start +then + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery + + # + # Disable px4io topic limiting + # + if [ $BOARD == fmuv1 ] + then + px4io limit 200 + else + px4io limit 400 + fi +else + # SOS + tone_alarm 6 +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 09c2d00d1..dc4be8055 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,5 +5,10 @@ if [ -d /fs/microsd ] then - sdlog start + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -a -b 16 + else + sdlog2 start -r 200 -a -b 16 + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 81184f363..73f1b3742 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -37,13 +37,3 @@ multirotor_att_control start # Start position control # multirotor_pos_control start - -# -# Start logging -# -if [ $BOARD == fmuv1 ] -then - sdlog2 start -r 50 -a -b 16 -else - sdlog2 start -r 200 -a -b 16 -fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b278..d92a3ce5e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -57,162 +57,165 @@ fi if [ $MODE == autostart ] then - -# -# Start terminal -# -if sercon -then - echo "USB connected" -else - # second attempt - sercon & -fi - -# -# Start the ORB (first app to start) -# -uorb start - -# -# Load microSD params -# -if ramtron start -then - param select /ramtron/params - if [ -f /ramtron/params ] + # + # Start terminal + # + if sercon then - param load /ramtron/params + echo "USB connected" + else + # second attempt + sercon & fi -else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] + + # + # Start the ORB (first app to start) + # + uorb start + + # + # Load microSD params + # + if ramtron start then - param load /fs/microsd/params + param select /ramtron/params + if [ -f /ramtron/params ] + then + param load /ramtron/params + fi + else + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + param load /fs/microsd/params + fi fi -fi - -# -# Start system state indicator -# -if rgbled start -then - echo "Using external RGB Led" -else - if blinkm start + + # + # Start system state indicator + # + if rgbled start then - blinkm systemstate + echo "Using external RGB Led" + else + if blinkm start + then + blinkm systemstate + fi fi -fi - -# -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) -# -if [ -f /fs/microsd/px4io.bin ] -then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + + # + # Start logging + # + sh /etc/init.d/rc.logging + + # + # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) + # + if [ -f /fs/microsd/px4io.bin ] then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + echo "No newer version, skipping upgrade." else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log - echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log + echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + fi fi fi -fi - -# -# Check if auto-setup from one of the standard scripts is wanted -# SYS_AUTOSTART = 0 means no autostart (default) -# -if param compare SYS_AUTOSTART 1 -then - sh /etc/init.d/01_fmu_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/02_io_quad_x - set MODE custom -fi - -if param compare SYS_AUTOSTART 8 -then - sh /etc/init.d/08_ardrone - set MODE custom -fi - -if param compare SYS_AUTOSTART 9 -then - sh /etc/init.d/09_ardrone_flow - set MODE custom -fi - -if param compare SYS_AUTOSTART 10 -then - sh /etc/init.d/10_io_f330 - set MODE custom -fi - -if param compare SYS_AUTOSTART 15 -then - sh /etc/init.d/15_io_tbs - set MODE custom -fi - -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/30_io_camflyer - set MODE custom -fi - -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/31_io_phantom - set MODE custom -fi - -# Try to get an USB console -nshterm /dev/ttyACM0 & - -# If none of the autostart scripts triggered, get a minimal setup -if [ $MODE == autostart ] -then - # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 - - # Start commander - commander start - - # Start px4io if present - if px4io start + + # + # Check if auto-setup from one of the standard scripts is wanted + # SYS_AUTOSTART = 0 means no autostart (default) + # + if param compare SYS_AUTOSTART 1 then - echo "PX4IO driver started" - else - if fmu mode_serial + sh /etc/init.d/01_fmu_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 2 + then + sh /etc/init.d/02_io_quad_x + set MODE custom + fi + + if param compare SYS_AUTOSTART 8 + then + sh /etc/init.d/08_ardrone + set MODE custom + fi + + if param compare SYS_AUTOSTART 9 + then + sh /etc/init.d/09_ardrone_flow + set MODE custom + fi + + if param compare SYS_AUTOSTART 10 + then + sh /etc/init.d/10_io_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 15 + then + sh /etc/init.d/15_io_tbs + set MODE custom + fi + + if param compare SYS_AUTOSTART 30 + then + sh /etc/init.d/30_io_camflyer + set MODE custom + fi + + if param compare SYS_AUTOSTART 31 + then + sh /etc/init.d/31_io_phantom + set MODE custom + fi + + # Try to get an USB console + nshterm /dev/ttyACM0 & + + # If none of the autostart scripts triggered, get a minimal setup + if [ $MODE == autostart ] + then + # Telemetry port is on both FMU boards ttyS1 + mavlink start -b 57600 -d /dev/ttyS1 + usleep 5000 + + # Start commander + commander start + + # Start px4io if present + if px4io start then - echo "FMU driver started" + echo "PX4IO driver started" + else + if fmu mode_serial + then + echo "FMU driver started" + fi fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - -fi - # End of autostart fi -- cgit v1.2.3 From 725bb7697c9fc85ac95fdadc9d2fd2ef5b9848f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 20:17:42 +0200 Subject: Minor fix in "set mode" command handling. --- src/modules/commander/commander.cpp | 13 ++++++------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f7ac24341..74cd5e36a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -323,11 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - union px4_custom_mode custom_mode; - custom_mode.data_float = cmd->param2; + uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -357,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb28af1a1..af43542da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -201,7 +201,7 @@ handle_message(mavlink_message_t *msg) custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.data_float; + vcmd.param2 = custom_mode.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; -- cgit v1.2.3 From 2075cba636eff6bb59aeefe0eee7eaa13276b16e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 22:13:49 +0200 Subject: Added H frame to docs --- Documentation/control_flow.graffle | 2166 ++++++++++++++++++++++++++++++++---- 1 file changed, 1975 insertions(+), 191 deletions(-) diff --git a/Documentation/control_flow.graffle b/Documentation/control_flow.graffle index 253523110..6188c4430 100644 --- a/Documentation/control_flow.graffle +++ b/Documentation/control_flow.graffle @@ -5,7 +5,7 @@ ApplicationVersion com.omnigroup.OmniGraffle - 139.16.0.171715 + 139.18.0.187838 CreationDate 2013-02-22 17:51:02 +0000 @@ -26,9 +26,9 @@ MasterSheets ModificationDate - 2013-04-20 15:38:47 +0000 + 2013-08-25 05:58:40 +0000 Modifier - Lorenz Meier + Lorenz NotesVisible NO OriginVisible @@ -55,7 +55,7 @@ NSPaperName string - iso-a3 + BADB3137-012D-43BB-AFDE-2C0069A874AD NSPaperSize @@ -123,7 +123,7 @@ Bounds - {{320.41170773935767, 646.55758982070449}, {100, 24}} + {{320.41170773792788, 646.55758667263535}, {100, 24}} Class ShapedGraphic FitText @@ -171,7 +171,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -193,8 +193,8 @@ 45 Points - {283.47949897905681, 658.66217570036315} - {459.99996984645378, 658.44980851233595} + {283.47949897630235, 658.66217426345122} + {459.99996984638915, 658.4498036008282} Style @@ -243,7 +243,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -289,7 +289,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -340,7 +340,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -389,7 +389,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -437,7 +437,7 @@ Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -461,8 +461,8 @@ 38 Points - {584.52245687751122, 125.49999965650382} - {584.56069426576869, 157.00000090441219} + {584.50076858662646, 125.50000000000021} + {584.50207726115275, 157.00000000001324} Style @@ -511,7 +511,7 @@ Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -532,8 +532,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 34 Points - {210.47275259888471, 495.87499981747118} - {210.43193555768113, 543.06250166479344} + {210.49312053369275, 495.87499999999562} + {210.48281498396668, 543.06249999958038} Style @@ -582,7 +582,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -602,8 +602,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 32 Points - {210.46968123779743, 580.06249992316259} - {210.44442316539605, 627.25000030102035} + {210.47945786724097, 580.0625} + {210.47913680605853, 627.24999999999989} Style @@ -637,8 +637,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 30 Points - {210.48620562018775, 690.25} - {210.49612530146715, 737.4375} + {210.48620103078346, 690.25} + {210.49611383706232, 737.4375} Style @@ -694,7 +694,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -731,7 +731,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -789,7 +789,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -801,7 +801,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Bounds - {{152.49994329565658, 397.00120984204113}, {116, 24}} + {{152.49998995675264, 397.00120984204113}, {116, 24}} Class ShapedGraphic FitText @@ -849,7 +849,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -877,8 +877,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} -1 Points - {584.49994992834695, 220} - {584.4997453697481, 348.6875000000008} + {584.49999370649277, 220} + {584.49999370649277, 348.6875} Style @@ -971,7 +971,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -991,8 +991,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 17 Points - {210.49997491180866, 385.6875} - {210.49991091996927, 432.875} + {210.49998995675264, 385.6875} + {210.49998995675264, 432.875} Style @@ -1029,7 +1029,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Points {283.49999999999511, 270.00001907176534} - {339.50000000000028, 270.00001907176534} + {339.49999999999994, 270.00001907176534} Style @@ -1098,8 +1098,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 13 Points - {210.49171354592596, 207} - {210.49498558851923, 238.4999999999998} + {210.49209551674531, 207} + {210.49601794208507, 238.5} Style @@ -1133,8 +1133,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT} 12 Points - {210.49498558851246, 138.5000000000002} - {210.48997117702493, 170.00000000001342} + {210.49601794208485, 138.5} + {210.49203588416967, 170} Style @@ -1190,7 +1190,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1234,7 +1234,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1278,7 +1278,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1322,7 +1322,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1360,7 +1360,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1395,7 +1395,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1430,7 +1430,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1675,7 +1675,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1774,7 +1774,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2145,7 +2145,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2536,7 +2536,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2927,7 +2927,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3361,7 +3361,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3404,7 +3404,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3791,7 +3791,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4178,7 +4178,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4565,7 +4565,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4963,7 +4963,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5062,7 +5062,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5421,7 +5421,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5780,7 +5780,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6175,7 +6175,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6660,7 +6660,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7019,7 +7019,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7406,7 +7406,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7793,7 +7793,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8300,7 +8300,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8404,7 +8404,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8795,7 +8795,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9166,7 +9166,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9537,7 +9537,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9928,7 +9928,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10367,7 +10367,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10779,7 +10779,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10883,7 +10883,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11270,7 +11270,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11629,7 +11629,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11988,7 +11988,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12375,7 +12375,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12810,7 +12810,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13262,6 +13262,55 @@ PX4IO or PX4FMU} Group Graphics + + Bounds + {{281.4621559838219, 891.87422762618871}, {48, 14}} + Class + ShapedGraphic + FitText + YES + Flow + Resize + ID + 1534 + Shape + Rectangle + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + Pad + 0 + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\b\fs24 \cf0 QUAD +\b0 H} + VerticalPad + 0 + + Wrap + NO + Class Group @@ -13269,11 +13318,11 @@ PX4IO or PX4FMU} Bounds - {{317.51000800051321, 240.42163882949882}, {50.666667938232422, 36}} + {{282.10284531231378, 729.4992324185539}, {50.666667938232422, 36}} Class ShapedGraphic ID - 1417 + 1536 Rotation 270 Shape @@ -13301,13 +13350,11 @@ PX4IO or PX4FMU} Bounds - {{306.84335619815613, 227.6666597724805}, {72, 72}} + {{271.43617794923421, 713.47324050519865}, {72, 72}} Class ShapedGraphic ID - 1418 - Rotation - 45 + 1537 Shape Rectangle Style @@ -13321,27 +13368,86 @@ PX4IO or PX4FMU} ID - 1416 + 1535 + Rotation + 315 Class Group Graphics + + Bounds + {{387.95096684220994, 833.99511602139876}, {24, 24}} + Class + ShapedGraphic + ID + 1539 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 4} + VerticalPad + 0 + + Class Group Graphics + + Class + LineGraphic + ID + 1541 + Points + + {447.56635253821332, 822.0678021003348} + {445.35664384700522, 823.21685061976291} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + Bounds - {{181.87174317649047, 313.76849034719959}, {24, 24}} + {{359.66328626413321, 875.75030562593793}, {16, 18.5}} Class ShapedGraphic + HFlip + YES ID - 1421 + 1542 + Rotation + 45 Shape - Circle + HorizontalTriangle Style shadow @@ -13352,110 +13458,1695 @@ PX4IO or PX4FMU} Text - Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 -\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} -{\colortbl;\red255\green255\blue255;} -\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc - -\f0\fs24 \cf0 6} VerticalPad 0 - VFlip - YES + Bounds + {{349.2634215034181, 795.53620935597417}, {101.5, 101.5}} Class - Group - Graphics - + ShapedGraphic + HFlip + YES + ID + 1543 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill - Class - LineGraphic - ID - 1423 - Points - - {159.49938242361688, 285.249576623557} - {161.10116522486757, 287.15677793899749} - - Style - - stroke - - HeadArrow - 0 - Legacy - - TailArrow - 0 - - + Draws + NO + shadow - Bounds - {{201.23696348262672, 364.71920821881605}, {16, 18.5}} - Class - ShapedGraphic - HFlip - YES - ID - 1424 - Rotation - 157.5 - Shape - HorizontalTriangle - Style - - shadow - - Draws - NO - - - Text - - VerticalPad - 0 - - VFlip - YES + Draws + NO + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{346.5134215034181, 792.78620935597417}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1544 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill - Bounds - {{143.41457890258161, 275.07218626448082}, {101.5, 101.5}} - Class - ShapedGraphic - HFlip - YES - ID - 1425 - Rotation - 67.5 - Shape - AdjustableArc - ShapeData - - endAngle - 72 - startAngle - 281 - - Style - - fill - - Draws - NO - - shadow - - Draws - NO - - - Text + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1540 + Rotation + 315 + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1546 + Points + + {352.51941963025371, 870.09214125245364} + {354.7291283214617, 868.94309273302531} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{424.42248590433371, 797.90963772685075}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1547 + Rotation + 225 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{349.32235066504887, 795.12373399681428}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1548 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{346.57235066504887, 792.37373399681428}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1549 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1545 + Rotation + 315 + + + Bounds + {{349.03928365032027, 795.08342417943641}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1550 + Rotation + 315 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.234512 + g + 0.673128 + r + 0.148947 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + + + ID + 1538 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{202.98738428569231, 833.92908910567212}, {24, 24}} + Class + ShapedGraphic + ID + 1552 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 2} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1554 + Points + + {167.72554658547227, 821.64822718882488} + {169.93525527668044, 822.79727570825287} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{239.62861285955245, 875.33073071442777}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1555 + Rotation + 135 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{164.52847762026795, 795.1166344444639}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1556 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{161.77847762026795, 792.3666344444639}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1557 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1553 + Rotation + 225 + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1559 + Points + + {262.41893329590295, 870.02611253847215} + {260.20922460469507, 868.87706401904404} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{174.51586702182317, 797.84360901286959}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1560 + Rotation + 315 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{164.11600226110755, 795.05770528283301}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1561 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{161.36600226110755, 792.30770528283301}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1562 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1558 + Rotation + 225 + VFlip + YES + + + Bounds + {{164.07569244372991, 795.01739580669891}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1563 + Rotation + 225 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.673128 + g + 0.534129 + r + 0.217952 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + ID + 1551 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{424.02444829866442, 666.20122319568361}, {24, 24}} + Class + ShapedGraphic + ID + 1565 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 1} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1567 + Points + + {388.76261059844421, 653.92036127883637} + {390.97231928965209, 655.06940979826459} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{460.66567687252439, 707.60286480443915}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1568 + Rotation + 135 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{385.56554163323983, 627.3887685344755}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1569 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{382.81554163323983, 624.6387685344755}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1570 + Rotation + 45 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1566 + Rotation + 225 + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1572 + Points + + {483.455997308875, 702.29824662848364} + {481.24628861766723, 701.14919810905553} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{395.55293103479517, 630.11574310288108}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1573 + Rotation + 315 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{385.15306627407972, 627.32983937284462}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1574 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + Bounds + {{382.40306627407972, 624.57983937284462}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1575 + Rotation + 225 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + VFlip + YES + + + ID + 1571 + Rotation + 225 + VFlip + YES + + + Bounds + {{385.11275645670196, 627.28952989671052}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1576 + Rotation + 225 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.673128 + g + 0.534129 + r + 0.217952 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + ID + 1564 + Rotation + 315 + + + Class + Group + Graphics + + + Bounds + {{170.01738471163631, 670.02445557113811}, {24, 24}} + Class + ShapedGraphic + ID + 1578 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 3} + VerticalPad + 0 + + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1580 + Points + + {229.63277040763995, 658.09714165007426} + {227.42306171643185, 659.24619016950237} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{141.72970413355978, 711.77964517567739}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1581 + Rotation + 45 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{131.32983937284456, 631.5655489057134}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1582 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{128.57983937284456, 628.8155489057134}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1583 + Rotation + 135 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1579 + Rotation + 315 + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1585 + Points + + {134.58583749968011, 706.12148080219276} + {136.79554619088805, 704.97243228276466} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{206.48890377375986, 633.93897727658998}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1586 + Rotation + 225 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + + + Bounds + {{131.38876853447516, 631.15307354655363}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1587 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + Bounds + {{128.63876853447516, 628.40307354655363}, {107, 107}} + Class + ShapedGraphic + HFlip + YES + ID + 1588 + Rotation + 315 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + TextRelativeArea + {{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}} + + + ID + 1584 + Rotation + 315 + + + Bounds + {{131.1057015197467, 631.11276372917564}, {101.82337649086284, 101.82337649086284}} + Class + ShapedGraphic + ID + 1589 + Rotation + 315 + Shape + Circle + Style + + fill + + Color + + a + 0.78 + b + 0.234512 + g + 0.673128 + r + 0.148947 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + Text + + VerticalPad + 0 + + + + ID + 1577 + Rotation + 315 + + + Bounds + {{253.2315878349163, 736.67782196944336}, {8, 134}} + Class + ShapedGraphic + ID + 1590 + Rotation + 225 + Shape + Rectangle + + + Bounds + {{371.51084102985902, 644.26864825135988}, {8, 134}} + Class + ShapedGraphic + ID + 1591 + Rotation + 240 + Shape + Rectangle + + + Bounds + {{353.64074716687509, 736.67780398678804}, {8, 134}} + Class + ShapedGraphic + ID + 1592 + Rotation + 315 + Shape + Rectangle + + + Bounds + {{234.00404905414581, 645.26862880510475}, {8, 134}} + Class + ShapedGraphic + ID + 1593 + Rotation + 300 + Shape + Rectangle + + + ID + 1533 + + + Class + Group + Graphics + + + Class + Group + Graphics + + + Bounds + {{317.51000800051321, 240.42163882949882}, {50.666667938232422, 36}} + Class + ShapedGraphic + ID + 1417 + Rotation + 270 + Shape + HorizontalTriangle + Style + + fill + + Color + + b + 0.13195 + g + 0.165669 + r + 1 + + + + Text + + VerticalPad + 0 + + + + Bounds + {{306.84335619815613, 227.6666597724805}, {72, 72}} + Class + ShapedGraphic + ID + 1418 + Rotation + 45 + Shape + Rectangle + Style + + stroke + + CornerRadius + 9 + + + + + ID + 1416 + + + Class + Group + Graphics + + + Class + Group + Graphics + + + Bounds + {{181.87174317649047, 313.76849034719959}, {24, 24}} + Class + ShapedGraphic + ID + 1421 + Shape + Circle + Style + + shadow + + Draws + NO + + + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 6} + VerticalPad + 0 + + VFlip + YES + + + Class + Group + Graphics + + + Class + LineGraphic + ID + 1423 + Points + + {159.49938242361688, 285.249576623557} + {161.10116522486757, 287.15677793899749} + + Style + + stroke + + HeadArrow + 0 + Legacy + + TailArrow + 0 + + + + + Bounds + {{201.23696348262672, 364.71920821881605}, {16, 18.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1424 + Rotation + 157.5 + Shape + HorizontalTriangle + Style + + shadow + + Draws + NO + + + Text + + VerticalPad + 0 + + VFlip + YES + + + Bounds + {{143.41457890258161, 275.07218626448082}, {101.5, 101.5}} + Class + ShapedGraphic + HFlip + YES + ID + 1425 + Rotation + 67.5 + Shape + AdjustableArc + ShapeData + + endAngle + 72 + startAngle + 281 + + Style + + fill + + Draws + NO + + shadow + + Draws + NO + + + Text VerticalPad 0 @@ -13744,7 +15435,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14135,7 +15826,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14526,7 +16217,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14928,7 +16619,7 @@ PX4IO or PX4FMU} Pad 0 Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14966,7 +16657,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15337,7 +17028,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15708,7 +17399,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16115,7 +17806,7 @@ PX4IO or PX4FMU} Text Text - {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + {\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16581,6 +18272,94 @@ PX4IO or PX4FMU} VPages 1 + + ActiveLayerIndex + 0 + AutoAdjust + + BackgroundGraphic + + Bounds + {{0, 0}, {806, 1132}} + Class + SolidGraphic + ID + 2 + Style + + shadow + + Draws + NO + + stroke + + Draws + NO + + + + BaseZoom + 0 + CanvasOrigin + {0, 0} + ColumnAlign + 1 + ColumnSpacing + 36 + DisplayScale + 1 0/72 in = 1.0000 in + GraphicsList + + GridInfo + + HPages + 1 + KeepToScale + + Layers + + + Lock + NO + Name + Ebene 1 + Print + YES + View + YES + + + LayoutInfo + + Animate + NO + circoMinDist + 18 + circoSeparation + 0.0 + layoutEngine + dot + neatoSeparation + 0.0 + twopiSeparation + 0.0 + + Orientation + 2 + PrintOnePage + + RowAlign + 1 + RowSpacing + 36 + SheetTitle + Arbeitsfläche 6 + UniqueID + 6 + VPages + 1 + SmartAlignmentGuidesActive YES @@ -16600,7 +18379,7 @@ PX4IO or PX4FMU} Frame - {{96, 56}, {1043, 822}} + {{170, 4}, {1043, 774}} ListView OutlineWidth @@ -16614,7 +18393,7 @@ PX4IO or PX4FMU} SidebarWidth 120 VisibleRegion - {{-51, -33}, {908, 683}} + {{-51, 366}, {908, 635}} Zoom 1 ZoomValues @@ -16644,6 +18423,11 @@ PX4IO or PX4FMU} 1 1 + + Arbeitsfläche 6 + 1 + 1 + -- cgit v1.2.3 From 548f322493fae95c41f3769192fa0c9b28d44d26 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:43:01 +0200 Subject: Added a simple unit test framework and initial testing some of the commander state machines. --- makefiles/config_px4fmu-v1_default.mk | 7 + makefiles/config_px4fmu-v2_default.mk | 6 + .../commander/commander_tests/commander_tests.cpp | 57 +++++ src/modules/commander/commander_tests/module.mk | 41 ++++ .../commander_tests/state_machine_helper_test.cpp | 248 +++++++++++++++++++++ .../commander_tests/state_machine_helper_test.h | 43 ++++ src/modules/test/foo.c | 4 - src/modules/test/module.mk | 4 - src/modules/unit_test/module.mk | 39 ++++ src/modules/unit_test/unit_test.cpp | 65 ++++++ src/modules/unit_test/unit_test.h | 93 ++++++++ 11 files changed, 599 insertions(+), 8 deletions(-) create mode 100644 src/modules/commander/commander_tests/commander_tests.cpp create mode 100644 src/modules/commander/commander_tests/module.mk create mode 100644 src/modules/commander/commander_tests/state_machine_helper_test.cpp create mode 100644 src/modules/commander/commander_tests/state_machine_helper_test.h delete mode 100644 src/modules/test/foo.c delete mode 100644 src/modules/test/module.mk create mode 100644 src/modules/unit_test/module.mk create mode 100644 src/modules/unit_test/unit_test.cpp create mode 100644 src/modules/unit_test/unit_test.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a556d0b07..245fe8415 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -90,6 +90,13 @@ MODULES += examples/flow_speed_control # MODULES += modules/sdlog2 +# +# Unit tests +# +MODULES += modules/unit_test +MODULES += modules/commander/commander_tests + + # # Library modules # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 20dbe717f..c5131262b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -83,6 +83,12 @@ MODULES += modules/multirotor_pos_control # MODULES += modules/sdlog2 +# +# Unit tests +# +MODULES += modules/unit_test +MODULES += modules/commander/commander_tests + # # Library modules # diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp new file mode 100644 index 000000000..f1a9674aa --- /dev/null +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * 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 commander_tests.cpp + * Commander unit tests. + * + */ + +#include + +#include "state_machine_helper_test.h" + +extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); + + +int commander_tests_main(int argc, char *argv[]) +{ + state_machine_helper_test(); + //state_machine_test(); + + return 0; +} diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk new file mode 100644 index 000000000..df9b7ac4b --- /dev/null +++ b/src/modules/commander/commander_tests/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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = commander_tests +SRCS = commander_tests.cpp \ + state_machine_helper_test.cpp \ + ../state_machine_helper.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp new file mode 100644 index 000000000..7101b455a --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 state_machine_helper_test.cpp + * System state machine unit test. + * + */ + +#include "state_machine_helper_test.h" + +#include "../state_machine_helper.h" +#include + +class StateMachineHelperTest : public UnitTest +{ +public: + StateMachineHelperTest(); + virtual ~StateMachineHelperTest(); + + virtual const char* run_tests(); + +private: + const char* arming_state_transition_test(); + const char* arming_state_transition_arm_disarm_test(); + const char* main_state_transition_test(); + const char* is_safe_test(); +}; + +StateMachineHelperTest::StateMachineHelperTest() { +} + +StateMachineHelperTest::~StateMachineHelperTest() { +} + +const char* +StateMachineHelperTest::arming_state_transition_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // Identical states. + status.arming_state = ARMING_STATE_INIT; + new_arming_state = ARMING_STATE_INIT; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + + // INIT to STANDBY. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = true; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("transition: init to standby", + TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("ready to arm", armed.ready_to_arm); + + // INIT to STANDBY, sensors not initialized. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = false; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("no transition: sensors not initialized", + TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("not ready to arm", !armed.ready_to_arm); + + return 0; +} + +const char* +StateMachineHelperTest::arming_state_transition_arm_disarm_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // TODO(sjwilks): ARM then DISARM. + return 0; +} + +const char* +StateMachineHelperTest::main_state_transition_test() +{ + struct vehicle_status_s current_state; + main_state_t new_main_state; + + // Identical states. + current_state.main_state = MAIN_STATE_MANUAL; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // AUTO to MANUAL. + current_state.main_state = MAIN_STATE_AUTO; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("transition changed: auto to manual", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to SEATBELT. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = true; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("tranisition: manual to seatbelt", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + + // MANUAL to SEATBELT, invalid local altitude. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = false; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("no transition: invalid local altitude", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to EASY. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = true; + new_main_state = MAIN_STATE_EASY; + mu_assert("transition: manual to easy", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + + // MANUAL to EASY, invalid local position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = false; + new_main_state = MAIN_STATE_EASY; + mu_assert("no transition: invalid position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to AUTO. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = true; + new_main_state = MAIN_STATE_AUTO; + mu_assert("transition: manual to auto", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + + // MANUAL to AUTO, invalid global position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = false; + new_main_state = MAIN_STATE_AUTO; + mu_assert("no transition: invalid global position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + return 0; +} + +const char* +StateMachineHelperTest::is_safe_test() +{ + struct vehicle_status_s current_state; + struct safety_s safety; + struct actuator_armed_s armed; + + armed.armed = false; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + + armed.armed = false; + armed.lockdown = true; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = false; + safety.safety_off = false; + mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + + return 0; +} + +const char* +StateMachineHelperTest::run_tests() +{ + mu_run_test(arming_state_transition_test); + mu_run_test(arming_state_transition_arm_disarm_test); + mu_run_test(main_state_transition_test); + mu_run_test(is_safe_test); + + return 0; +} + +void +state_machine_helper_test() +{ + StateMachineHelperTest* test = new StateMachineHelperTest(); + test->UnitTest::print_results(test->run_tests()); +} + diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h new file mode 100644 index 000000000..339b58d22 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 state_machine_helper_test.h + */ + +#ifndef STATE_MACHINE_HELPER_TEST_H_ +#define STATE_MACHINE_HELPER_TEST_ + +void state_machine_helper_test(); + +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ \ No newline at end of file diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c deleted file mode 100644 index ff6af031f..000000000 --- a/src/modules/test/foo.c +++ /dev/null @@ -1,4 +0,0 @@ -int test_main(void) -{ - return 0; -} \ No newline at end of file diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk deleted file mode 100644 index abf80eedf..000000000 --- a/src/modules/test/module.mk +++ /dev/null @@ -1,4 +0,0 @@ - -MODULE_NAME = test -SRCS = foo.c - diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk new file mode 100644 index 000000000..f00b0f592 --- /dev/null +++ b/src/modules/unit_test/module.mk @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the unit test library. +# + +SRCS = unit_test.cpp + diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp new file mode 100644 index 000000000..64ee544a2 --- /dev/null +++ b/src/modules/unit_test/unit_test.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 unit_test.cpp + * A unit test library. + * + */ + +#include "unit_test.h" + +#include + + +UnitTest::UnitTest() +{ +} + +UnitTest::~UnitTest() +{ +} + +void +UnitTest::print_results(const char* result) +{ + if (result != 0) { + warnx("Failed: %s:%d", mu_last_test(), mu_line()); + warnx("%s", result); + } else { + warnx("ALL TESTS PASSED"); + warnx(" Tests run : %d", mu_tests_run()); + warnx(" Assertion : %d", mu_assertion()); + } +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h new file mode 100644 index 000000000..5d4c3e46d --- /dev/null +++ b/src/modules/unit_test/unit_test.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * 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 unit_test.h + * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). + * + */ + +#ifndef UNIT_TEST_H_ +#define UNIT_TEST_ + +#include + + +class __EXPORT UnitTest +{ + +public: +#define xstr(s) str(s) +#define str(s) #s +#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } + +INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_assertion) +INLINE_GLOBAL(int, mu_line) +INLINE_GLOBAL(const char*, mu_last_test) + +#define mu_assert(message, test) \ + do \ + { \ + if (!(test)) \ + return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ + else \ + mu_assertion()++; \ + } while (0) + + +#define mu_run_test(test) \ +do \ +{ \ + const char *message; \ + mu_last_test() = #test; \ + mu_line() = __LINE__; \ + message = test(); \ + mu_tests_run()++; \ + if (message) \ + return message; \ +} while (0) + + +public: + UnitTest(); + virtual ~UnitTest(); + + virtual const char* run_tests() = 0; + virtual void print_results(const char* result); +}; + + + +#endif /* UNIT_TEST_H_ */ \ No newline at end of file -- cgit v1.2.3 From e25f2ff44f9579da3e5bef1e6d1baa3822ec40df Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Aug 2013 22:54:31 +0200 Subject: Whitespace and formatting cleanup. --- makefiles/config_px4fmu-v1_default.mk | 1 - src/modules/commander/commander_tests/commander_tests.cpp | 8 +++----- src/modules/commander/commander_tests/module.mk | 2 +- .../commander/commander_tests/state_machine_helper_test.cpp | 1 - src/modules/commander/commander_tests/state_machine_helper_test.h | 3 ++- src/modules/unit_test/unit_test.h | 2 +- 6 files changed, 7 insertions(+), 10 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 245fe8415..bb0c1550f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -96,7 +96,6 @@ MODULES += modules/sdlog2 MODULES += modules/unit_test MODULES += modules/commander/commander_tests - # # Library modules # diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index f1a9674aa..6e72cf0d9 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -1,10 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes + * Author: Simon Wilks * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +34,8 @@ /** * @file commander_tests.cpp - * Commander unit tests. + * Commander unit tests. Run the tests as follows: + * nsh> commander_tests * */ diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk index df9b7ac4b..4d10275d1 100644 --- a/src/modules/commander/commander_tests/module.mk +++ b/src/modules/commander/commander_tests/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 7101b455a..40bedd9f3 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -245,4 +245,3 @@ state_machine_helper_test() StateMachineHelperTest* test = new StateMachineHelperTest(); test->UnitTest::print_results(test->run_tests()); } - diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 339b58d22..10a68e602 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,4 +41,4 @@ void state_machine_helper_test(); -#endif /* STATE_MACHINE_HELPER_TEST_H_ */ \ No newline at end of file +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 5d4c3e46d..3020734f4 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -90,4 +90,4 @@ public: -#endif /* UNIT_TEST_H_ */ \ No newline at end of file +#endif /* UNIT_TEST_H_ */ -- cgit v1.2.3 From 07f7fd1585dab0a0256b0e8dda48ea1a1bd65388 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 25 Aug 2013 22:26:47 -0700 Subject: Fix the firmware build rules so that we always know how to build all the firmwares and thus we can have dependencies between FMU and IO firmware handled a little more sensibly. --- Makefile | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/Makefile b/Makefile index cbf0da6c9..83a6f3ce9 100644 --- a/Makefile +++ b/Makefile @@ -40,14 +40,16 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ include $(PX4_BASE)makefiles/setup.mk # -# Canned firmware configurations that we build. +# Canned firmware configurations that we (know how to) build. # -CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +CONFIGS ?= $(KNOWN_CONFIGS) # -# Boards that we build NuttX export kits for. +# Boards that we (know how to) build NuttX export kits for. # -BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +BOARDS ?= $(KNOWN_BOARDS) # # Debugging @@ -87,10 +89,11 @@ endif # # Built products # -STAGED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) -FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: $(STAGED_FIRMWARES) +all: $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -122,7 +125,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: $(FIRMWARE_GOAL) # -# Make FMU firmwares depend on pre-packaged IO binaries. +# Make FMU firmwares depend on the corresponding IO firmware. # # This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency # and forces the _default config in all cases. There has to be a better way to do this... -- cgit v1.2.3 From c5731bbc3f29361f3d50ecc54d44a521d2441a48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 09:12:17 +0200 Subject: TAKEOFF implemented for multirotors, added altitude check to waypoint navigation. --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 2 +- src/modules/commander/commander.cpp | 14 ++- src/modules/commander/state_machine_helper.cpp | 1 + src/modules/mavlink/orb_listener.c | 2 +- src/modules/mavlink/waypoints.c | 40 ++++---- .../multirotor_pos_control.c | 101 +++++++++++++-------- .../position_estimator_inav_main.c | 3 +- src/modules/uORB/topics/vehicle_control_mode.h | 2 + src/modules/uORB/topics/vehicle_global_position.h | 3 +- src/modules/uORB/topics/vehicle_local_position.h | 2 + 10 files changed, 101 insertions(+), 69 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f30..33879892e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -325,7 +325,7 @@ void KalmanNav::updatePublications() _pos.vx = vN; _pos.vy = vE; _pos.vz = vD; - _pos.hdg = psi; + _pos.yaw = psi; // attitude publication _att.timestamp = _pubTimeStamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74cd5e36a..39d74e37a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -210,7 +210,7 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -1214,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) } /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode); + transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1572,7 +1572,7 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; @@ -1592,8 +1592,12 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + /* check if takeoff completed */ + if (local_pos->z < -5.0f && !status.condition_landed) { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } else { + res = TRANSITION_NOT_CHANGED; + } } else { if (current_status->condition_landed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 80ee3db23..fe7e8cc53 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + control_mode->auto_state = current_status->navigation_state; navigation_state_changed = true; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index dcdc03281..53d86ec00 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -341,7 +341,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a17..16a7c2d35 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) */ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) { - //TODO: implement for z once altidude contoller is implemented - static uint16_t counter; -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; + double current_x_rad = wp->x / 180.0 * M_PI; + double current_y_rad = wp->y / 180.0 * M_PI; double x_rad = lat / 180.0 * M_PI; double y_rad = lon / 180.0 * M_PI; @@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float const double radius_earth = 6371000.0; - return radius_earth * c; + float dxy = radius_earth * c; + float dz = alt - wp->z; + + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; @@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ // XXX TODO } - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - + if (dist >= 0.f && dist <= orbit) { wpm->pos_reached = true; - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } + // check if required yaw reached + float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); + float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); + if (fabsf(yaw_err) < 0.05f) { + wpm->yaw_reached = true; + } } //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); @@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ bool time_elapsed = false; - if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } - } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; @@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); // } - check_waypoints_reached(now, global_position , local_position); + check_waypoints_reached(now, global_position, local_position); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a6ebeeacb..e65792c76 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_int_xy = true; bool was_armed = false; bool reset_integral = true; + bool reset_auto_pos = true; hrt_abstime t_prev = 0; - /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + const float takeoff_alt_default = 10.0f; float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -414,50 +416,76 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, use global setpoint */ - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - global_pos_sp_reproject = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_reproject) { - /* update global setpoint projection */ - global_pos_sp_reproject = false; + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = -takeoff_alt_default; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + } - if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } - } else { - if (!local_pos_sp_valid) { - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + att_sp.yaw_body = global_pos_sp.yaw; + reset_auto_pos = true; + } - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; } /* reset setpoints after non-manual modes */ @@ -575,6 +603,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_int_z = true; reset_int_xy = true; global_pos_sp_reproject = true; + reset_auto_pos = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 1e20f9de9..af6a704a2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -591,6 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.landed = landed; + local_pos.yaw = att.yaw; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -609,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { @@ -623,6 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 4f4db5dbc..e15ddde26 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -82,6 +82,8 @@ struct vehicle_control_mode_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ + + uint8_t auto_state; // TEMP navigation state for AUTO modes }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 822c323cf..0fc0ed5c9 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -72,8 +72,7 @@ struct vehicle_global_position_s float vx; /**< Ground X velocity, m/s in NED */ float vy; /**< Ground Y velocity, m/s in NED */ float vz; /**< Ground Z velocity, m/s in NED */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 26e8f335b..31a0e632b 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -67,6 +67,8 @@ struct vehicle_local_position_s float vx; /**< Ground X Speed (Latitude), m/s in NED */ float vy; /**< Ground Y Speed (Longitude), m/s in NED */ float vz; /**< Ground Z Speed (Altitude), m/s in NED */ + /* Heading */ + float yaw; /* Reference position in GPS / WGS84 frame */ bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ -- cgit v1.2.3 From dfde02c82507d183fdc16aa2d45cb8d9cdf6ff0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 11:53:52 +0200 Subject: Startup scripts fixup, fixed unmatched dependencies --- ROMFS/px4fmu_common/init.d/30_io_camflyer | 9 +++++++-- ROMFS/px4fmu_common/init.d/31_io_phantom | 7 ++++++- ROMFS/px4fmu_common/init.d/rc.multirotor | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 11 +++-------- 4 files changed, 21 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index c9d5d6632..6a0bd4da8 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,6 +1,6 @@ #!nsh -cho "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" +echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" # # Load default params for this platform @@ -49,6 +49,11 @@ px4io limit 100 # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface (depends on orb) @@ -64,4 +69,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 0deffe3f1..718313862 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -44,6 +44,11 @@ commander start # Start the sensors # sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging # # Start GPS interface @@ -59,4 +64,4 @@ kalman_demo start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -control_demo start +fw_att_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 73f1b3742..e3638e3d1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -8,6 +8,11 @@ # sh /etc/init.d/rc.sensors +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + # # Start the commander. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6a82acdcc..c4abd07d2 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -103,11 +103,9 @@ then blinkm systemstate fi fi - - # - # Start logging - # - sh /etc/init.d/rc.logging + + # Try to get an USB console + nshterm /dev/ttyACM0 & # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) @@ -183,9 +181,6 @@ then set MODE custom fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then -- cgit v1.2.3 From 3157285254381827bc0312bfb413ff6823feee3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 13:26:01 +0200 Subject: Increased the number of max files descriptors considerably --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c6..56ad46c3f 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_STREAMS=50 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 3e2a48108..f99973aec 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_STREAMS=50 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 -- cgit v1.2.3 From 00a2a0370eedf84f68dcda5995f4e34495aaf887 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 12:43:36 +0200 Subject: accelerometer_calibration fix --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 82df7c37d..7a7fa6f4e 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -298,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ - int64_t still_time = 2000000; + hrt_abstime still_time = 2000000; struct pollfd fds[1]; fds[0].fd = sub_sensor_combined; fds[0].events = POLLIN; @@ -342,7 +342,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { t_timeout = t + timeout; } else { /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { + if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } -- cgit v1.2.3 From bf9282c9882882a5fc4adf680a6ecc5e655bb0e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:10 +0200 Subject: position_estimator_inav: requre EPH < 5m to set GPS reference --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index af6a704a2..bc6e0b020 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { -- cgit v1.2.3 From baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:55 +0200 Subject: commander: do AUTO_MISSION after takeoff --- src/modules/commander/commander.cpp | 54 ++++++++++++++++++------------------- 1 file changed, 26 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d74e37a..d90008633 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* check if takeoff completed */ - if (local_pos->z < -5.0f && !status.condition_landed) { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } else { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -5.0f || status.condition_landed) { res = TRANSITION_NOT_CHANGED; + break; } + } + /* check again, state can be changed */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } - - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } + + } else { + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } } else { -- cgit v1.2.3 From 7326f8a4215fe736aedd2e2cdb2ab51d06e20f80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:53:30 +0200 Subject: multirotor_pos_control: fixes, set local_position_sp.yaw --- .../multirotor_pos_control.c | 25 ++++++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e65792c76..385892f04 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -405,30 +405,38 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for loiter after position controlled mode */ local_pos_sp_valid = control_mode.flag_control_position_enabled; + reset_auto_pos = true; + /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; } else { /* non-manual mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; } @@ -462,6 +470,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -472,15 +482,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } - - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } - att_sp.yaw_body = global_pos_sp.yaw; reset_auto_pos = true; } @@ -493,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_z = true; } + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; -- cgit v1.2.3 From e88d63ef272124e8c0ee9574506d14866feadb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 15:03:58 +0200 Subject: Increased USB buffer size to cope with fast transfers --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index b6f14490e..3e02c204e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" -- cgit v1.2.3 From b29d13347a10156bf1690b67938e2850d4e1e4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 22:08:56 +0200 Subject: position_estimator_inav: reset reference altitude on arming. --- .../position_estimator_inav_main.c | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index bc6e0b020..ef13ade88 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s + +static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const uint32_t updates_counter_len = 1000000; +static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); - uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* acceleration in NED frame */ float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; @@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { // new ground level baro_alt0 += sonar_corr; - warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); @@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + } + /* accel bias correction, now only for Z * not strictly correct, but stable and works */ accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; @@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } + flag_armed = armed.armed; } warnx("exiting."); -- cgit v1.2.3 From bfd0444cb30bf2db3f19a6a1c052d85c2270a090 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 07:49:36 +0200 Subject: Revert "Increased the number of max files descriptors considerably" This reverts commit 3157285254381827bc0312bfb413ff6823feee3b. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 56ad46c3f..a7a6725c6 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,8 +405,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 -CONFIG_NFILE_STREAMS=50 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index f99973aec..3e2a48108 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,8 +451,8 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 -CONFIG_NFILE_STREAMS=50 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 -- cgit v1.2.3 From 7b42d7a0470aaa50e9a8ec5852007ea52165a7ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 07:50:15 +0200 Subject: Made number of streams more reasonable --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a7a6725c6..b8b98b149 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -406,7 +406,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 3e2a48108..e507c89ba 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -452,7 +452,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 -- cgit v1.2.3 From 94d8ec4a1c1f052a50f4871db7445fb6454057d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 09:48:22 +0200 Subject: Calibration message cleanup --- .../commander/accelerometer_calibration.cpp | 17 +++++++-------- src/modules/commander/gyro_calibration.cpp | 15 ++++++++----- src/modules/commander/mag_calibration.cpp | 25 +++++++++++++++------- 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a7fa6f4e..e1616acd0 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -219,7 +219,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool done = true; char str[60]; int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); + str_ptr = sprintf(str, "keep the vehicle still in one of these axes:"); unsigned old_done_count = done_count; done_count = 0; for (int i = 0; i < 6; i++) { @@ -236,22 +236,21 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float if (done) break; - mavlink_log_info(mavlink_fd, str); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) return ERROR; if (data_collected[orient]) { - sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); continue; } // sprintf(str, - mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + str_ptr = sprintf(str, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -265,7 +264,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float float accel_T[3][3]; int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } @@ -337,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "still..."); + mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; } else { @@ -352,7 +351,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); + mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } @@ -364,7 +363,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); + mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); return -1; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 9cd2f3a19..33566d4e5 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,7 @@ static const int ERROR = -1; int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); const int calibration_count = 5000; @@ -84,6 +84,8 @@ int do_gyro_calibration(int mavlink_fd) close(fd); + unsigned poll_errcount = 0; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -93,16 +95,19 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { + if (poll_ret > 0) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); gyro_offset[0] += raw.gyro_rad_s[0]; gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 42f0190c7..e38616027 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -61,7 +61,7 @@ static const int ERROR = -1; int do_mag_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -123,6 +123,10 @@ int do_mag_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + + unsigned poll_errcount = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -137,7 +141,7 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); @@ -158,7 +162,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); - if (poll_ret) { + if (poll_ret > 0) { orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; @@ -190,11 +194,16 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + return ERROR; } + + } float sphere_x; @@ -276,7 +285,7 @@ int do_mag_calibration(int mavlink_fd) (double)mscale.y_scale, (double)mscale.z_scale); mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; -- cgit v1.2.3 From 665a23259269d870e958543c6e6517323793c1dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 10:15:17 +0200 Subject: More calibration polishing --- .../commander/accelerometer_calibration.cpp | 23 +++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index e1616acd0..30ac07e33 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -217,20 +217,23 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float while (true) { bool done = true; - char str[60]; - int str_ptr; - str_ptr = sprintf(str, "keep the vehicle still in one of these axes:"); unsigned old_done_count = done_count; done_count = 0; + for (int i = 0; i < 6; i++) { if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; - } else { - done_count++; } } + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!data_collected[0]) ? "x- " : "", + (!data_collected[0]) ? "y+ " : "", + (!data_collected[0]) ? "y- " : "", + (!data_collected[0]) ? "z+ " : "", + (!data_collected[0]) ? "z- " : ""); + if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); @@ -242,19 +245,17 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float return ERROR; if (data_collected[orient]) { - sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); continue; } - // sprintf(str, mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; tune_neutral(); } -- cgit v1.2.3 From 9c58d2c5c6ef96f9ece7f62d5f02d01d8b1e316e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Aug 2013 18:07:31 +1000 Subject: airspeed: retry initial I2C probe 4 times this fixes a problem with detecting a MS4525D0 at boot --- src/drivers/airspeed/airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5a8157deb..277d8249a 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -146,7 +146,14 @@ out: int Airspeed::probe() { - return measure(); + /* on initial power up the device needs more than one retry + for detection. Once it is running then retries aren't + needed + */ + _retries = 4; + int ret = measure(); + _retries = 0; + return ret; } int -- cgit v1.2.3 From 33c73429098fee0650bdf2042a2c85e18975afca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 10:36:43 +0200 Subject: Minor fixes for calibration, UI language much more readable now --- src/modules/commander/accelerometer_calibration.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 30ac07e33..ed6707f04 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -228,11 +228,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", - (!data_collected[0]) ? "x- " : "", - (!data_collected[0]) ? "y+ " : "", - (!data_collected[0]) ? "y- " : "", - (!data_collected[0]) ? "z+ " : "", - (!data_collected[0]) ? "z- " : ""); + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); if (old_done_count != done_count) mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); -- cgit v1.2.3 From b9d6981cee9878158435b9b1daa955d5b25c0389 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 13:40:18 +0200 Subject: multirotor_att_control: yaw control bug fixed --- .../multirotor_att_control_main.c | 31 ++++++++++++---------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 09955ec50..b3669d9ff 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -147,8 +147,6 @@ mc_thread_main(int argc, char *argv[]) warnx("starting"); /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; bool control_yaw_position = true; bool reset_yaw_sp = true; bool failsafe_first_time = true; @@ -213,6 +211,11 @@ mc_thread_main(int argc, char *argv[]) /* set flag to safe value */ control_yaw_position = true; + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ @@ -234,10 +237,12 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; + } else if (control_mode.flag_control_manual_enabled) { /* manual input */ if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { @@ -279,17 +284,8 @@ mc_thread_main(int argc, char *argv[]) } else { failsafe_first_time = true; - /* control yaw in all manual / assisted modes */ - /* set yaw if arming or switching to attitude stabilized mode */ - - if (!flag_control_manual_enabled || !flag_control_attitude_enabled || !control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* only move setpoint if manual input is != 0 */ - - // TODO review yaw restpoint reset - if ((manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) && manual.throttle > min_takeoff_throttle) { + /* only move yaw setpoint if manual input is != 0 and not landed */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; rates_sp.yaw = manual.yaw; @@ -340,7 +336,14 @@ mc_thread_main(int argc, char *argv[]) rates_sp.thrust = manual.throttle; rates_sp.timestamp = hrt_absolute_time(); } + + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } /* check if we should we reset integrals */ -- cgit v1.2.3 From 3380d40a7d9a8d1946510fab42abdc7a3a6f1525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Aug 2013 15:37:04 +0200 Subject: Tighter configs to save RAM --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a6f914a64..feb4a393d 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=24 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e507c89ba..e1bc867e5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=24 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 -- cgit v1.2.3 From 864c1d048c6d9390d6bdf5a8058d48df9d36e973 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 20:16:51 +0200 Subject: Revert "Tighter configs to save RAM" This reverts commit 3380d40a7d9a8d1946510fab42abdc7a3a6f1525. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index feb4a393d..a6f914a64 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=24 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e1bc867e5..e507c89ba 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=24 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 -- cgit v1.2.3 From 66c61fbe96e11ee7099431a8370d84f862543810 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 23:08:00 +0200 Subject: Full failsafe rewrite. --- src/modules/commander/commander.cpp | 587 ++++++++++----------- src/modules/commander/commander_params.c | 2 +- src/modules/commander/state_machine_helper.cpp | 30 +- src/modules/commander/state_machine_helper.h | 6 +- src/modules/mavlink/mavlink.c | 6 +- .../multirotor_att_control_main.c | 98 ++-- .../multirotor_pos_control.c | 59 ++- .../position_estimator_inav_main.c | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 10 files changed, 394 insertions(+), 413 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d90008633..a548f943e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,8 +147,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* timout until lowlevel failsafe */ -static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; @@ -199,6 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode int commander_thread_main(int argc, char *argv[]); void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); + void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -206,17 +205,20 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); + void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); + int commander_main(int argc, char *argv[]) { @@ -271,7 +273,8 @@ void usage(const char *reason) exit(1); } -void print_status() { +void print_status() +{ warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ @@ -279,33 +282,40 @@ void print_status() { struct vehicle_status_s state; orb_copy(ORB_ID(vehicle_status), state_sub, &state); - const char* armed_str; + const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: - armed_str = "INIT"; - break; - case ARMING_STATE_STANDBY: - armed_str = "STANDBY"; - break; - case ARMING_STATE_ARMED: - armed_str = "ARMED"; - break; - case ARMING_STATE_ARMED_ERROR: - armed_str = "ARMED_ERROR"; - break; - case ARMING_STATE_STANDBY_ERROR: - armed_str = "STANDBY_ERROR"; - break; - case ARMING_STATE_REBOOT: - armed_str = "REBOOT"; - break; - case ARMING_STATE_IN_AIR_RESTORE: - armed_str = "IN_AIR_RESTORE"; - break; - default: - armed_str = "ERR: UNKNOWN STATE"; - break; + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + + default: + armed_str = "ERR: UNKNOWN STATE"; + break; } @@ -326,7 +336,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -415,6 +425,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } else { /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -478,9 +489,6 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); @@ -599,12 +607,6 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; - /* To remember when last notification was sent */ - uint64_t last_print_control_time = 0; - - enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; - bool armed_previous = false; - bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; @@ -665,7 +667,6 @@ int commander_thread_main(int argc, char *argv[]) int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -800,12 +801,15 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + } else { mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } @@ -908,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX should we still allow to arm with critical battery? //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } + status_changed = true; } @@ -943,11 +948,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -963,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) */ if (!home_position_set && gps_position.fix_type >= 3 && - (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate @@ -1013,9 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) } } - status.rc_signal_cutting_off = false; status.rc_signal_lost = false; - status.rc_signal_lost_interval = 0; transition_result_t res; // store all transitions results here @@ -1027,10 +1025,10 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); @@ -1040,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) } else { stick_off_counter++; } + } else { stick_off_counter = 0; } @@ -1087,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { @@ -1097,122 +1096,14 @@ int commander_thread_main(int argc, char *argv[]) } } else { - - /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { - // TODO remove debug code - if (!status.rc_signal_cutting_off) { - warnx("Reason: not rc_signal_cutting_off\n"); - - } else { - warnx("last print time: %llu\n", last_print_control_time); - } - - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (status.rc_signal_lost_interval > 1000000) { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; - status.failsave_lowlevel = true; status_changed = true; } } } - /* END mode switch */ - /* END RC state check */ - - // TODO check this - /* state machine update for offboard control */ - if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? - - // /* decide about attitude control flag, enable in att/pos/vel */ - // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - // /* decide about rate control flag, enable it always XXX (for now) */ - // bool rates_ctrl_enabled = true; - - // /* set up control mode */ - // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - // state_changed = true; - // } - - // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - // current_status.flag_control_rates_enabled = rates_ctrl_enabled; - // state_changed = true; - // } - - // /* handle the case where offboard control signal was regained */ - // if (!current_status.offboard_control_signal_found_once) { - // current_status.offboard_control_signal_found_once = true; - // /* enable offboard control, disable manual input */ - // current_status.flag_control_manual_enabled = false; - // current_status.flag_control_offboard_enabled = true; - // state_changed = true; - // tune_positive(); - - // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - // } else { - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - // state_changed = true; - // tune_positive(); - // } - // } - - status.offboard_control_signal_weak = false; - status.offboard_control_signal_lost = false; - status.offboard_control_signal_lost_interval = 0; - - // XXX check if this is correct - /* arm / disarm on request */ - if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - } - - } else { - - /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { - status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (status.offboard_control_signal_lost_interval > 100000) { - status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_positive(); - - /* kill motors after timeout */ - if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { - status.failsave_lowlevel = true; - status_changed = true; - } - } - } - } - /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); @@ -1230,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { status_changed = false; } @@ -1288,10 +1180,6 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - /* store old modes to detect and act on state transitions */ - battery_warning_previous = status.battery_warning; - armed_previous = armed.armed; - fflush(stdout); counter++; @@ -1343,6 +1231,7 @@ void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (armed->armed) { /* armed, solid */ @@ -1352,11 +1241,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); + } else { /* not ready to arm, blink at 10Hz */ if (leds_counter % 2 == 0) led_toggle(LED_BLUE); } + #endif if (changed) { @@ -1369,50 +1260,60 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - for (i=0; i<3; i++) { + for (i = 0; i < 3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; + pattern.duration[i * 2] = 200; + + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; } + if (status->condition_global_position_valid) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 1000; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 1000; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; + } else { - for (i=3; i<6; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 100; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 100; + for (i = 3; i < 6; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 100; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 100; } - pattern.color[6*2] = RGBLED_COLOR_OFF; - pattern.duration[6*2] = 700; + + pattern.color[6 * 2] = RGBLED_COLOR_OFF; + pattern.duration[6 * 2] = 700; } } else { - for (i=0; i<3; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 200; + for (i = 0; i < 3; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.duration[i * 2] = 200; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 200; } + /* not ready to arm, blink at 10Hz */ } @@ -1423,6 +1324,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->load > 0.95f) { if (leds_counter % 2 == 0) led_toggle(LED_AMBER); + } else { led_off(LED_AMBER); } @@ -1572,70 +1474,124 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) +check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - case MAIN_STATE_AUTO: - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (status->main_state == MAIN_STATE_AUTO) { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status.condition_landed) { + if (local_pos->z > -5.0f || status->condition_landed) { res = TRANSITION_NOT_CHANGED; - break; } } - /* check again, state can be changed */ - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + if (res != TRANSITION_NOT_CHANGED) { + /* check again, state can be changed */ + if (status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + + } else { + /* not landed */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* switch to MISSION in air when no RC control */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } + } + + } else { + /* disarmed, always switch to AUTO_READY */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + } + + } else { + /* manual control modes */ + if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { + /* switch to failsafe mode */ + bool manual_control_old = control_mode->flag_control_manual_enabled; + + if (!status->condition_landed) { + /* in air: try to hold position */ + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + + } else { + /* landed: don't try to hold position but land (if taking off) */ + res = TRANSITION_DENIED; + } + + if (res == TRANSITION_DENIED) { + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + } + + control_mode->flag_control_manual_enabled = false; + + if (res == TRANSITION_NOT_CHANGED && manual_control_old) { + /* mark navigation state as changed to force immediate flag publishing */ + set_navigation_state_changed(); + res = TRANSITION_CHANGED; + } + + if (res == TRANSITION_CHANGED) { + if (control_mode->flag_control_position_enabled) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); } else { - /* switch to mission in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + if (status->condition_landed) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + } } } + } else { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - break; + switch (status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - default: - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + break; + + default: + break; + } + } } return res; @@ -1644,27 +1600,32 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case VEHICLE_CMD_RESULT_ACCEPTED: tune_positive(); - break; - case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - tune_negative(); - break; - default: - break; + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + + default: + break; } } @@ -1720,11 +1681,13 @@ void *commander_low_prio_loop(void *arg) usleep(1000000); /* reboot */ systemreset(false); + } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(1000000); /* reboot to bootloader */ systemreset(true); + } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } @@ -1732,83 +1695,85 @@ void *commander_low_prio_loop(void *arg) } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; + + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - int calib_ret = ERROR; + int calib_ret = ERROR; - /* try to go to INIT/PREFLIGHT arming state */ + /* try to go to INIT/PREFLIGHT arming state */ - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - break; - } + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_gyro_calibration(mavlink_fd); + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_mag_calibration(mavlink_fd); + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_accel_calibration(mavlink_fd); + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_airspeed_calibration(mavlink_fd); - } + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - if (calib_ret == OK) - tune_positive(); - else - tune_negative(); + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } } + + break; } - break; - } default: answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); @@ -1817,7 +1782,7 @@ void *commander_low_prio_loop(void *arg) /* send any requested ACKs */ if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { /* send acknowledge command */ // XXX TODO } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6832fc5ce..0a1703b2e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include #include -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fe7e8cc53..674f3feda 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * return ret; } -bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) { // System is safe if: // 1) Not armed @@ -276,12 +276,12 @@ check_main_state_changed() } transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) +navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_status->navigation_state) { + if (new_navigation_state == status->navigation_state) { ret = TRANSITION_NOT_CHANGED; } else { @@ -296,6 +296,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_STABILIZE: @@ -307,6 +308,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_ALTHOLD: @@ -318,6 +320,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_VECTOR: @@ -329,6 +332,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_READY: @@ -340,12 +344,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -354,6 +359,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -367,6 +373,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: @@ -378,6 +385,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_RTL: @@ -389,12 +397,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LAND: /* deny transitions from landed state */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -403,6 +412,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -412,8 +422,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } if (ret == TRANSITION_CHANGED) { - current_status->navigation_state = new_navigation_state; - control_mode->auto_state = current_status->navigation_state; + status->navigation_state = new_navigation_state; + control_mode->auto_state = status->navigation_state; navigation_state_changed = true; } } @@ -433,6 +443,12 @@ check_navigation_state_changed() } } +void +set_navigation_state_changed() +{ + navigation_state_changed = true; +} + /** * Transition from one hil state to another */ diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a38c2497e..1641b6f60 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,10 +68,12 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); bool check_navigation_state_changed(); +void set_navigation_state_changed(); + int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 0a506b1a9..d7b0fa9c7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -240,11 +240,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u **/ /* set calibration state */ - if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_INIT + if (v_status.arming_state == ARMING_STATE_INIT || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index b3669d9ff..04582f2a4 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Implementation of multirotor attitude control main loop. * * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -63,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -74,8 +77,6 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. - __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; @@ -102,7 +103,8 @@ mc_thread_main(int argc, char *argv[]) memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -114,6 +116,8 @@ mc_thread_main(int argc, char *argv[]) int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* * Do not rate-limit the loop to prevent aliasing @@ -136,7 +140,6 @@ mc_thread_main(int argc, char *argv[]) orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); @@ -149,12 +152,6 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; - bool failsafe_first_time = true; - - /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); - float failsafe_throttle = 0.0f; - param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +173,6 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -205,6 +201,13 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } + /* get a local copy of status */ + orb_check(status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), status_sub, &status); + } + /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -244,47 +247,18 @@ mc_thread_main(int argc, char *argv[]) /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (control_mode.failsave_highlevel) { - failsafe_first_time = false; - - if (!control_mode.flag_control_velocity_enabled) { - /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - } - - if (!control_mode.flag_control_climb_rate_enabled) { - /* don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - } + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (failsafe_first_time) { + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ reset_yaw_sp = true; } } else { - failsafe_first_time = true; - - /* only move yaw setpoint if manual input is != 0 and not landed */ + /* only move yaw setpoint if manual input is != 0 */ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; @@ -294,18 +268,17 @@ mc_thread_main(int argc, char *argv[]) } else { control_yaw_position = true; } + } - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } - } /* reset yaw setpint to current position if needed */ @@ -324,7 +297,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* publish the controller output */ + /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -342,6 +315,17 @@ mc_thread_main(int argc, char *argv[]) } } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + } + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 385892f04..8227f76c5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,10 +415,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; - } else { - /* non-manual mode, use global setpoint */ + } else if (control_mode.flag_control_auto_enabled) { + /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; @@ -428,21 +429,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); - } - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { - if (reset_auto_pos) { - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; - local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; - att_sp.yaw_body = att.yaw; - reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { @@ -457,6 +450,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (global_pos_sp_reproject) { /* update global setpoint projection */ global_pos_sp_reproject = false; + if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.lat * 1e-7; @@ -470,6 +464,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; @@ -489,6 +484,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } } + reset_auto_pos = true; } @@ -496,9 +492,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_pos_sp_reproject = true; } - /* reset setpoints after non-manual modes */ + /* reset setpoints after AUTO mode */ reset_sp_xy = true; reset_sp_z = true; + + } else { + /* no control, loiter or stay on ground */ + if (local_pos.landed) { + /* landed: move setpoint down */ + /* in air: hold altitude */ + if (local_pos_sp.z < 5.0f) { + /* set altitude setpoint to 5m under ground, + * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ + local_pos_sp.z = 5.0f; + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + } + + reset_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + } + } } /* publish local position setpoint */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ef13ade88..4a4572b09 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e15ddde26..093c6775d 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -80,9 +80,7 @@ struct vehicle_control_mode_s bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ - + bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9f55bb874..43218eac4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -174,8 +174,6 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ -// uint64_t failsave_highlevel_begin; TO BE COMPLETED main_state_t main_state; /**< main state machine */ navigation_state_t navigation_state; /**< navigation state machine */ @@ -207,22 +205,13 @@ struct vehicle_status_s bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception is terminally lost */ - bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ - uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool rc_signal_lost; /**< true if RC reception lost */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; - - bool preflight_calibration; - - bool system_emergency; - /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; -- cgit v1.2.3 From d28f5ac03fc9e73cf26c8afa1ed701ca5af0df08 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 09:14:38 +0200 Subject: Updated IO firmware upgrade strategy and locations --- ROMFS/px4fmu_common/init.d/rcS | 22 ++++++------------- src/drivers/px4io/px4io.cpp | 8 +++---- src/drivers/px4io/px4io_uploader.cpp | 41 ++++++++++++++++++------------------ src/drivers/px4io/uploader.h | 2 +- 4 files changed, 33 insertions(+), 40 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4abd07d2..6da03402e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,25 +108,17 @@ then nshterm /dev/ttyACM0 & # - # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) + # Upgrade PX4IO firmware # - if [ -f /fs/microsd/px4io.bin ] + if px4io update then - echo "PX4IO Firmware found. Checking Upgrade.." - if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + if [ -d /fs/microsd ] then - echo "No newer version, skipping upgrade." - else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log - then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log - else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log - echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" - fi + echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log fi + + # Allow IO to safely kick back to app + usleep 200000 fi # diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 15873f79b..392bc9f0a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2096,10 +2096,10 @@ px4io_main(int argc, char *argv[]) fn[1] = nullptr; } else { - fn[0] = "/fs/microsd/px4io.bin"; - fn[1] = "/etc/px4io.bin"; - fn[2] = "/fs/microsd/px4io2.bin"; - fn[3] = "/etc/px4io2.bin"; + fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[1] = "/etc/extras/px4io-v1_default.bin"; + fn[2] = "/fs/microsd/px4io.bin"; + fn[3] = "/fs/microsd/px4io2.bin"; fn[4] = nullptr; } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index c7ce60b5e..7db28ecad 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -82,6 +82,27 @@ PX4IO_Uploader::upload(const char *filenames[]) #error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload #endif + /* allow an early abort and look for file first */ + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + + log("using firmware from %s", filenames[i]); + filename = filenames[i]; + break; + } + + if (filename == NULL) { + log("no firmware found"); + close(_io_fd); + _io_fd = -1; + return -ENOENT; + } + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); if (_io_fd < 0) { @@ -106,26 +127,6 @@ PX4IO_Uploader::upload(const char *filenames[]) return -EIO; } - for (unsigned i = 0; filenames[i] != nullptr; i++) { - _fw_fd = open(filenames[i], O_RDONLY); - - if (_fw_fd < 0) { - log("failed to open %s", filenames[i]); - continue; - } - - log("using firmware from %s", filenames[i]); - filename = filenames[i]; - break; - } - - if (filename == NULL) { - log("no firmware found"); - close(_io_fd); - _io_fd = -1; - return -ENOENT; - } - struct stat st; if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 135202dd1..a4a8a88fe 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 100); int sync(); int get_info(int param, uint32_t &val); int erase(); -- cgit v1.2.3 From ad732ee3a146b40c2b600eb78f804086105a4c57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:00:32 +1000 Subject: free perf counters in driver destructor this prevents drivers that probe on one bus then instantiate on another from leaving behind stale/duplicate perf counters --- src/drivers/airspeed/airspeed.cpp | 5 +++++ src/drivers/hmc5883/hmc5883.cpp | 5 +++++ src/drivers/ms5611/ms5611.cpp | 6 ++++++ 3 files changed, 16 insertions(+) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 277d8249a..e54f5b3ab 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -106,6 +106,11 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d77f03bb7..1a337ca3a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -359,6 +359,11 @@ HMC5883::~HMC5883() /* free any existing reports */ if (_reports != nullptr) delete[] _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b3..b572e042c 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -225,6 +225,12 @@ MS5611::~MS5611() if (_reports != nullptr) delete[] _reports; + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + delete _interface; } -- cgit v1.2.3 From fdbc09e2a53281b8dda7c48676dcf695a79ba373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:31:27 +1000 Subject: avoid counters going above limit in INCREMENT() when using INCREMENT() the counter would temporarily read equal to limit, which could cause an issue if the task is preempted. (this macro should be in a common header, though which header?) --- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/bma180/bma180.cpp | 2 +- src/drivers/hmc5883/hmc5883.cpp | 2 +- src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/ms5611/ms5611.cpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 89dfb22d7..b87494b40 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -165,5 +165,5 @@ protected: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index cfb625670..079b5d21c 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -234,7 +234,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) BMA180::BMA180(int bus, spi_dev_e device) : diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1a337ca3a..a5229b237 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -311,7 +311,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* * Driver 'main' command. diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5e0a2119a..e6d765e13 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -300,7 +300,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index cf5f8d94c..05d6f1881 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -421,7 +421,7 @@ private: /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c5f49fb36..f83416993 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -184,7 +184,7 @@ private: }; /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* * Driver 'main' command. diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index b572e042c..4e43f19c5 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -77,7 +77,7 @@ static const int ERROR = -1; #endif /* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) /* helper macro for arithmetic - returns the square of the argument */ #define POW2(_x) ((_x) * (_x)) -- cgit v1.2.3 From 935ed2fe49370e5eafe3b5445eda2c5714162216 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:03:43 +1000 Subject: meas_airspeed: don't use stale/bad data in airspeed reading also fixed handling of perf counters on error --- src/drivers/meas_airspeed/meas_airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 666bd30e6..b1cb2b3d8 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -162,6 +162,8 @@ MEASAirspeed::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } @@ -169,9 +171,14 @@ MEASAirspeed::collect() if (status == 2) { log("err: stale data"); - + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } else if (status == 3) { log("err: fault"); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); -- cgit v1.2.3 From 76a9e34e08573ff67ad34eb0251e3a7bdefd0406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Aug 2013 18:26:21 +1000 Subject: I2C airspeed driver needs 2 retries this prevents I2C transfer errors every few seconds with the meas_airspeed driver --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index e54f5b3ab..1ec61eb60 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -157,7 +157,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 0; + _retries = 2; return ret; } -- cgit v1.2.3 From 0fb3be64eace32c9be8a17dab3d1cc3ee0459f9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 11:18:10 +0200 Subject: More verbosity on RC cal fail in sensors app --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e857b1c2f..a9c49c441 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -673,7 +673,7 @@ Sensors::parameters_update() if (!isfinite(_parameters.scaling_factor[i]) || _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; rc_valid = false; -- cgit v1.2.3 From 4c3c09990262bd0f9e9574dd950da62bb7b432a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 Aug 2013 15:53:45 +1000 Subject: USB: set attributes for bus power, no remote wakeup this may help the USB bus providing the full 500mA on some systems --- nuttx-configs/px4fmu-v1/nsh/defconfig | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a6f914a64..68ab4da32 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -538,9 +538,10 @@ CONFIG_USBDEV=y # # CONFIG_USBDEV_ISOCHRONOUS is not set # CONFIG_USBDEV_DUALSPEED is not set -CONFIG_USBDEV_SELFPOWERED=y -# CONFIG_USBDEV_BUSPOWERED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set -- cgit v1.2.3 From b7ee1d34294b536c6a92c8e72455f24684a5db4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:34:49 +0200 Subject: Prevented an analog airspeed corner case from happening --- src/modules/sensors/sensor_params.c | 3 ++- src/modules/sensors/sensors.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd2a6318e..16bfc8094 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a9c49c441..88a6bebb8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -301,6 +301,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; int board_rotation; int external_mag_rotation; @@ -348,6 +349,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; @@ -617,6 +619,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -784,6 +787,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1266,9 +1270,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor -- cgit v1.2.3 From 49ef30b834888b91e7238662b23d651addf3b4b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:58:29 +0200 Subject: Reworked how start scripts work, relying on io detect now --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 2 +- ROMFS/px4fmu_common/init.d/08_ardrone | 2 +- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 2 +- ROMFS/px4fmu_common/init.d/10_io_f330 | 43 +++++++++++-------- ROMFS/px4fmu_common/init.d/15_io_tbs | 2 +- ROMFS/px4fmu_common/init.d/30_io_camflyer | 2 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 2 +- ROMFS/px4fmu_common/init.d/40_io_segway | 2 +- ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 2 +- ROMFS/px4fmu_common/init.d/rc.boarddetect | 66 ------------------------------ ROMFS/px4fmu_common/init.d/rc.hil | 10 ----- 11 files changed, 35 insertions(+), 100 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.boarddetect diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x index f57e4bd68..b106a974f 100644 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -25,7 +25,7 @@ then param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index eb9f82f77..f6d82a5ec 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 44fbb79b7..794342a0b 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 7b6509bf8..5af4f97b5 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" # # Load default params for this platform @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - param save /fs/microsd/params + param save fi # @@ -32,24 +32,30 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io +set EXIT_ON_END no # -# Set PWM values for DJI ESCs +# Start and configure PX4IO or FMU interface # -px4io idle 900 900 900 900 -px4io min 1200 1200 1200 1200 -px4io max 1800 1800 1800 1800 +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi # # Load mixer @@ -65,3 +71,8 @@ pwm -u 400 -m 0xff # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs index b4f063e52..3fce39403 100644 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_P 0.2 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 6a0bd4da8..390649f89 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 718313862..45563310c 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -11,7 +11,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 2890f43be..4b7ed5286 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -9,7 +9,7 @@ then # TODO param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index 2c8218013..938663f0c 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -25,7 +25,7 @@ then param set RC_SCALE_YAW 3 param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect deleted file mode 100644 index f233e51df..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.boarddetect +++ /dev/null @@ -1,66 +0,0 @@ -#!nsh -# -# If we are still in flight mode, work out what airframe -# configuration we have and start up accordingly. -# -if [ $MODE != autostart ] -then - echo "[init] automatic startup cancelled by user script" -else - echo "[init] detecting attached hardware..." - - # - # Assume that we are PX4FMU in standalone mode - # - set BOARD PX4FMU - - # - # Are we attached to a PX4IOAR (AR.Drone carrier board)? - # - if boardinfo test name PX4IOAR - then - set BOARD PX4IOAR - if [ -f /etc/init.d/rc.PX4IOAR ] - then - echo "[init] reading /etc/init.d/rc.PX4IOAR" - usleep 500 - sh /etc/init.d/rc.PX4IOAR - fi - else - echo "[init] PX4IOAR not detected" - fi - - # - # Are we attached to a PX4IO? - # - if boardinfo test name PX4IO - then - set BOARD PX4IO - if [ -f /etc/init.d/rc.PX4IO ] - then - echo "[init] reading /etc/init.d/rc.PX4IO" - usleep 500 - sh /etc/init.d/rc.PX4IO - fi - else - echo "[init] PX4IO not detected" - fi - - # - # Looks like we are stand-alone - # - if [ $BOARD == PX4FMU ] - then - echo "[init] no expansion board detected" - if [ -f /etc/init.d/rc.standalone ] - then - echo "[init] reading /etc/init.d/rc.standalone" - sh /etc/init.d/rc.standalone - fi - fi - - # - # We may not reach here if the airframe-specific script exits the shell. - # - echo "[init] startup done." -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index a843b7ffb..eccb2b767 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -13,16 +13,6 @@ mavlink start -b 230400 -d /dev/ttyS1 # Create a fake HIL /dev/pwm_output interface hil mode_pwm -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -- cgit v1.2.3 From a48be0446bf78f73d7550864150c1cf3de4dafa3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:58:53 +0200 Subject: Added IO detect command to be smart about what to start before actually doing it --- src/drivers/px4io/px4io.cpp | 65 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 392bc9f0a..dda1b825c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -119,10 +119,17 @@ public: /** * Initialize the PX4IO class. * - * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers. + * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); + /** + * Detect if a PX4IO is connected. + * + * Only validate if there is a PX4IO to talk to. + */ + virtual int detect(); + /** * IO Control handler. * @@ -475,6 +482,29 @@ PX4IO::~PX4IO() g_dev = nullptr; } +int +PX4IO::detect() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + + return 0; +} + int PX4IO::init() { @@ -1894,6 +1924,7 @@ start(int argc, char *argv[]) if (OK != g_dev->init()) { delete g_dev; + g_dev = nullptr; errx(1, "driver init failed"); } @@ -1920,6 +1951,35 @@ start(int argc, char *argv[]) exit(0); } +void +detect(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(0, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->detect()) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver detect did not succeed"); + } + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + exit(0); +} + void bind(int argc, char *argv[]) { @@ -2079,6 +2139,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); + if (!strcmp(argv[1], "detect")) + detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { -- cgit v1.2.3 From feb4dad9e1c8e766cac93b55437c8234493ed71b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 15:22:24 +0200 Subject: Fixed IO detect message --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dda1b825c..beeb01974 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -497,10 +497,10 @@ PX4IO::detect() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); if (protocol != PX4IO_PROTOCOL_VERSION) { - log("protocol/firmware mismatch"); - mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + log("IO not installed"); return -1; } + log("IO found"); return 0; } -- cgit v1.2.3 From 0731d331bfbee88f91fa4737ec77c0a66ce171f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 15:50:06 +0200 Subject: ROMFS reshuffling / cleanup, in sync with QGC --- ROMFS/px4fmu_common/init.d/01_fmu_quad_x | 64 ----------------------- ROMFS/px4fmu_common/init.d/02_io_quad_x | 47 ----------------- ROMFS/px4fmu_common/init.d/100_mpx_easystar | 68 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/10_dji_f330 | 78 +++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/10_io_f330 | 78 ----------------------------- ROMFS/px4fmu_common/init.d/11_dji_f450 | 78 +++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/15_io_tbs | 67 ------------------------- ROMFS/px4fmu_common/init.d/15_tbs_discovery | 78 +++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/16_3dr_iris | 78 +++++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/30_io_camflyer | 5 +- ROMFS/px4fmu_common/init.d/31_io_phantom | 5 +- ROMFS/px4fmu_common/init.d/rcS | 27 +++++----- 12 files changed, 400 insertions(+), 273 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/01_fmu_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/02_io_quad_x create mode 100644 ROMFS/px4fmu_common/init.d/100_mpx_easystar create mode 100644 ROMFS/px4fmu_common/init.d/10_dji_f330 delete mode 100644 ROMFS/px4fmu_common/init.d/10_io_f330 create mode 100644 ROMFS/px4fmu_common/init.d/11_dji_f450 delete mode 100644 ROMFS/px4fmu_common/init.d/15_io_tbs create mode 100644 ROMFS/px4fmu_common/init.d/15_tbs_discovery create mode 100644 ROMFS/px4fmu_common/init.d/16_3dr_iris diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x deleted file mode 100644 index b106a974f..000000000 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ /dev/null @@ -1,64 +0,0 @@ -#!nsh - -echo "[init] 01_fmu_quad_x: PX4FMU Quad X with PWM outputs" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start PWM output -# -fmu mode_pwm - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm -u 400 -m 0xff - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x deleted file mode 100644 index a37c26ad1..000000000 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ /dev/null @@ -1,47 +0,0 @@ -#!nsh - -echo "[init] 02_io_quad_x: PX4FMU+PX4IO Quad X with PWM outputs" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm -u 400 -m 0xff - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar new file mode 100644 index 000000000..e1cefdb99 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -0,0 +1,68 @@ +#!nsh + +echo "[init] Multiplex Easystar" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start and configure PX4IO interface +# +sh /etc/init.d/rc.io + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the commander +# +commander start + +# +# Start the sensors +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 new file mode 100644 index 000000000..5af4f97b5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 deleted file mode 100644 index 5af4f97b5..000000000 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ /dev/null @@ -1,78 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_RCLOSS_THR 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm -u 400 -m 0xff - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 new file mode 100644 index 000000000..5af4f97b5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs deleted file mode 100644 index 3fce39403..000000000 --- a/ROMFS/px4fmu_common/init.d/15_io_tbs +++ /dev/null @@ -1,67 +0,0 @@ -#!nsh - -echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery quad" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 - param set MC_RCLOSS_THR 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Set PWM values for DJI ESCs -# -px4io idle 900 900 900 900 -px4io min 1180 1180 1180 1180 -px4io max 1800 1800 1800 1800 - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm -u 400 -m 0xff - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery new file mode 100644 index 000000000..4e7406b66 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] Team Blacksheep Discovery Quad" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris new file mode 100644 index 000000000..5bcaaaac1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -0,0 +1,78 @@ +#!nsh + +echo "[init] 3DR Iris Quad" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Set PWM output frequency +# +pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 390649f89..f7e653362 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -61,12 +61,13 @@ sh /etc/init.d/rc.logging gps start # -# Start the attitude estimator (depends on orb) +# Start the attitude and position estimator # -kalman_demo start +att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 45563310c..e1e609927 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -56,12 +56,13 @@ sh /etc/init.d/rc.logging gps start # -# Start the attitude estimator +# Start the attitude and position estimator # -kalman_demo start +att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6da03402e..3ef6eb75e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -125,17 +125,6 @@ then # Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # - if param compare SYS_AUTOSTART 1 - then - sh /etc/init.d/01_fmu_quad_x - set MODE custom - fi - - if param compare SYS_AUTOSTART 2 - then - sh /etc/init.d/02_io_quad_x - set MODE custom - fi if param compare SYS_AUTOSTART 8 then @@ -151,13 +140,25 @@ then if param compare SYS_AUTOSTART 10 then - sh /etc/init.d/10_io_f330 + sh /etc/init.d/10_dji_f330 + set MODE custom + fi + + if param compare SYS_AUTOSTART 11 + then + sh /etc/init.d/11_dji_f450 set MODE custom fi if param compare SYS_AUTOSTART 15 then - sh /etc/init.d/15_io_tbs + sh /etc/init.d/15_tbs_discovery + set MODE custom + fi + + if param compare SYS_AUTOSTART 16 + then + sh /etc/init.d/16_3dr_iris set MODE custom fi -- cgit v1.2.3 From 756d67f2de759cb8d1e4be26deb108fc4e32899d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 16:38:09 +0200 Subject: Removed non-helpful verbosity --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index beeb01974..1fa3fbbfb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1969,7 +1969,7 @@ detect(int argc, char *argv[]) if (OK != g_dev->detect()) { delete g_dev; g_dev = nullptr; - errx(1, "driver detect did not succeed"); + exit(1); } if (g_dev != nullptr) { -- cgit v1.2.3 From 5ef7e71bb0ca81551622fb5089b34c46770c0706 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 16:42:43 +0200 Subject: More graceful startup messages --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3ef6eb75e..56b068cca 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -191,13 +191,13 @@ then commander start # Start px4io if present - if px4io start + if px4io detect then - echo "PX4IO driver started" + px4io start else if fmu mode_serial then - echo "FMU driver started" + echo "FMU driver (no PWM) started" fi fi -- cgit v1.2.3 From 007f4d79cbd7edec6fb69e4556f35bac8dce72f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 Aug 2013 16:51:02 +0200 Subject: param MC_RCLOSS_THR removed from scripts --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 1 - ROMFS/px4fmu_common/init.d/11_dji_f450 | 1 - ROMFS/px4fmu_common/init.d/15_tbs_discovery | 1 - ROMFS/px4fmu_common/init.d/16_3dr_iris | 1 - 4 files changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 5af4f97b5..fb141e8a7 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 5af4f97b5..fb141e8a7 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_P 2.0 diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index 4e7406b66..1fcde3e27 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 5.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index 5bcaaaac1..fa2cb1feb 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -16,7 +16,6 @@ then param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 param set MC_ATT_P 5.0 - param set MC_RCLOSS_THR 0.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 -- cgit v1.2.3 From f2f66432d76dad2587d3e8089d0c728eb8fdf2ae Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 08:49:21 -0700 Subject: Grab UTC time from GPS --- src/drivers/gps/ubx.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b579db715..ba5d14cc4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -42,13 +42,14 @@ * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ - -#include -#include -#include +#include #include +#include +#include #include -#include +#include +#include + #include #include #include @@ -452,7 +453,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME,&ts); +#endif + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); -- cgit v1.2.3 From 53813d44a0c6322e46e72c0439e0265ddea76e9e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 Aug 2013 21:41:52 +0200 Subject: sdlog2: "landed" flag logging --- src/modules/sdlog2/sdlog2.c | 9 +++++---- src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2b21bb5a5..e83fb7dd3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -890,13 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state; - log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d99892fe2..4eeb65a87 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -155,6 +155,7 @@ struct log_STAT_s { float battery_current; float battery_remaining; uint8_t battery_warning; + uint8_t landed; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -263,7 +264,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), -- cgit v1.2.3 From b986607a750db74e05c60d09b048971bb0cfb18f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 18:32:16 -0700 Subject: Add defines for RC15 params (16 channels total) minor cleanup of rc sanity check code --- src/modules/sensors/sensor_params.c | 7 +++++++ src/modules/sensors/sensors.cpp | 23 ++++++++++++++--------- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 16bfc8094..dd86f3e2e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -158,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + + PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 88a6bebb8..e98c4d548 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -660,7 +660,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -670,18 +672,21 @@ Sensors::parameters_update() param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f) ) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; } - + else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ -- cgit v1.2.3 From 55cfa5152c5e3ca3148eee776ec4467dd53eeca3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:41:59 +0200 Subject: Made low/critical warnings consisting --- src/drivers/blinkm/blinkm.cpp | 4 ++-- src/modules/commander/commander.cpp | 16 ++++++++-------- src/modules/uORB/topics/vehicle_status.h | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 490002254..2361f4dd1 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -541,7 +541,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -553,7 +553,7 @@ BlinkM::led() led_color_8 = LED_YELLOW; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a548f943e..e90300aff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -890,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; } @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; if (armed.armed) { @@ -1160,12 +1160,12 @@ int commander_thread_main(int argc, char *argv[]) if (tune_arm() == OK) arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1258,10 +1258,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { @@ -1272,10 +1272,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else if (armed->ready_to_arm) { for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 43218eac4..1c184d3a7 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -153,9 +153,9 @@ enum VEHICLE_TYPE { }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** -- cgit v1.2.3 From 9bcfe49cff7b8fce6dfbeac7f8233e554672ebff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:45:33 +0200 Subject: Changed RGBLED behaviour, breathe mode when disarmed and ready to arm --- src/drivers/drv_rgbled.h | 2 + src/drivers/rgbled/rgbled.cpp | 46 ++++++++++++++++-- src/modules/commander/commander.cpp | 93 ++++++++++++++----------------------- 3 files changed, 77 insertions(+), 64 deletions(-) diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 3c8bdec5d..07c6186dd 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -78,6 +78,7 @@ /** set pattern */ #define RGBLED_SET_PATTERN _RGBLEDIOC(7) + /* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless @@ -115,6 +116,7 @@ typedef enum { RGBLED_MODE_BLINK_SLOW, RGBLED_MODE_BLINK_NORMAL, RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_BREATHE, RGBLED_MODE_PATTERN } rgbled_mode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 05f079ede..feb8f1c6c 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -96,6 +96,11 @@ private: rgbled_mode_t _mode; rgbled_pattern_t _pattern; + float _brightness; + uint8_t _r; + uint8_t _g; + uint8_t _b; + bool _should_run; bool _running; int _led_interval; @@ -104,6 +109,7 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); void set_pattern(rgbled_pattern_t *pattern); + void set_brightness(float brightness); static void led_trampoline(void *arg); void led(); @@ -128,6 +134,10 @@ RGBLED::RGBLED(int bus, int rgbled) : _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), _running(false), + _brightness(1.0f), + _r(0), + _g(0), + _b(0), _led_interval(0), _counter(0) { @@ -191,7 +201,6 @@ int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; - switch (cmd) { case RGBLED_SET_RGB: /* set the specified RGB values */ @@ -246,6 +255,15 @@ RGBLED::led() else set_on(false); break; + case RGBLED_MODE_BREATHE: + if (_counter >= 30) + _counter = 0; + if (_counter <= 15) { + set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f)); + } else { + set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f)); + } + break; case RGBLED_MODE_PATTERN: /* don't run out of the pattern array and stop if the next frame is 0 */ if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) @@ -294,7 +312,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; - case RGBLED_COLOR_DIM_RED: // red + case RGBLED_COLOR_DIM_RED: // red set_rgb(90,0,0); break; case RGBLED_COLOR_DIM_YELLOW: // yellow @@ -306,7 +324,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_DIM_GREEN: // green set_rgb(0,90,0); break; - case RGBLED_COLOR_DIM_BLUE: // blue + case RGBLED_COLOR_DIM_BLUE: // blue set_rgb(0,0,90); break; case RGBLED_COLOR_DIM_WHITE: // white @@ -347,6 +365,12 @@ RGBLED::set_mode(rgbled_mode_t mode) _should_run = true; _led_interval = 100; break; + case RGBLED_MODE_BREATHE: + _should_run = true; + set_on(true); + _counter = 0; + _led_interval = 1000/15; + break; case RGBLED_MODE_PATTERN: _should_run = true; set_on(true); @@ -377,6 +401,13 @@ RGBLED::set_pattern(rgbled_pattern_t *pattern) set_mode(RGBLED_MODE_PATTERN); } +void +RGBLED::set_brightness(float brightness) { + + _brightness = brightness; + set_rgb(_r, _g, _b); +} + int RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) { @@ -413,7 +444,12 @@ RGBLED::set_on(bool on) int RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) { - const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; + /* save the RGB values in case we want to change the brightness later */ + _r = r; + _g = g; + _b = b; + + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; return transfer(msg, sizeof(msg), nullptr, 0); } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e90300aff..776cd5766 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -689,6 +689,8 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + toggle_status_leds(&status, &armed, true); + /* now initialized */ commander_initialized = true; thread_running = true; @@ -1252,72 +1254,45 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - int i; - rgbled_pattern_t pattern; - memset(&pattern, 0, sizeof(pattern)); + /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { - /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[0] = 1000; - + rgbled_set_mode(RGBLED_MODE_ON); } else if (armed->ready_to_arm) { - for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[i * 2] = 200; - - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; - } - - if (status->condition_global_position_valid) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 1000; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; + rgbled_set_mode(RGBLED_MODE_BREATHE); + } else { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + } - } else { - for (i = 3; i < 6; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 100; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 100; - } - pattern.color[6 * 2] = RGBLED_COLOR_OFF; - pattern.duration[6 * 2] = 700; - } - - } else { - for (i = 0; i < 3; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i * 2] = 200; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 200; - } + } - /* not ready to arm, blink at 10Hz */ + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + switch (status->battery_warning) { + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + default: + break; + } + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + default: + break; } - - rgbled_set_pattern(&pattern); } /* give system warnings on error LED, XXX maybe add memory usage warning too */ -- cgit v1.2.3 From 5146dd8ff80f6cff127fbdac27f4cb92e5954924 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:09:31 +0200 Subject: position_estimator_inav: critical bug fixed --- .../position_estimator_inav_main.c | 41 ++++++++++++---------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4a4572b09..88057b323 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,26 +429,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ if (!ref_xy_inited) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; - - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + /* require EPH < 10m */ + if (gps.eph_m < 10.0f) { + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } + } else { + ref_xy_init_start = 0; } } -- cgit v1.2.3 From 3a00def1899223514ea0f9bd6bd567ac11d02f7e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:11:24 +0200 Subject: commander: switch to AUTO_READY or AUTO_MISSION immediately, don't try to stay on ground --- src/modules/commander/commander.cpp | 84 +++++++++++++------------- src/modules/commander/commander_params.c | 2 +- src/modules/commander/state_machine_helper.cpp | 23 +++---- 3 files changed, 52 insertions(+), 57 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 776cd5766..8ddd86d03 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,7 @@ static uint64_t last_print_mode_reject_time = 0; /* if connected via USB */ static bool on_usb_power = false; +static float takeoff_alt = 5.0f; /* tasks waiting for low prio thread */ typedef enum { @@ -492,9 +493,10 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); /* welcome user */ - warnx("[commander] starting"); + warnx("starting"); /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -733,8 +735,11 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status_changed = true; - /* Re-check RC calibration */ + /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check()); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); } } @@ -1253,7 +1258,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { @@ -1263,8 +1267,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } - - } if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { @@ -1455,54 +1457,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->main_state == MAIN_STATE_AUTO) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status->condition_landed) { - res = TRANSITION_NOT_CHANGED; + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; } } - - if (res != TRANSITION_NOT_CHANGED) { - /* check again, state can be changed */ - if (status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + /* switch to AUTO mode */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* not landed */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - /* switch to MISSION in air when no RC control */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - } + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0a1703b2e..f22dac0c1 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include #include -PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 674f3feda..3cef10185 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t break; case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LOITER: -- cgit v1.2.3 From 61936412f3ccf9aff5a12d47b8c5fe34ff1a04fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 15:40:28 +0200 Subject: Speeded up IO startup, needs review --- ROMFS/px4fmu_common/init.d/rcS | 6 +++++- src/drivers/px4io/uploader.h | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 56b068cca..982e638fb 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -110,8 +110,12 @@ then # # Upgrade PX4IO firmware # - if px4io update + if px4io detect then + echo "PX4IO running, not upgrading" + else + echo "Attempting to upgrade PX4IO" + px4io update if [ -d /fs/microsd ] then echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index a4a8a88fe..135202dd1 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 100); + int get_sync(unsigned timeout = 1000); int sync(); int get_info(int param, uint32_t &val); int erase(); -- cgit v1.2.3 From e2b602339adef80af84d6d396adc1962b1f86826 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 17:05:21 +0200 Subject: Cleanup of detect return --- src/drivers/px4io/px4io.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1fa3fbbfb..026b87949 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1966,18 +1966,16 @@ detect(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "driver alloc failed"); - if (OK != g_dev->detect()) { - delete g_dev; - g_dev = nullptr; - exit(1); - } + ret = g_dev->detect() - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } + delete g_dev; + g_dev = nullptr; - exit(0); + if (ret) + /* nonzero, error */ + exit(1); + else + exit(0); } void -- cgit v1.2.3 From 2b62497fb5dd83db17b1d2851f686049841faa7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Aug 2013 17:19:03 +0200 Subject: Fixed build error --- src/drivers/px4io/px4io.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 026b87949..47baa5770 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -890,7 +890,6 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::set_min_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -903,7 +902,6 @@ PX4IO::set_min_values(const uint16_t *vals, unsigned len) int PX4IO::set_max_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -1370,7 +1368,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != num_values) { + if (ret != (int)num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } @@ -1393,7 +1391,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v } int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); - if (ret != num_values) { + if (ret != (int)num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } @@ -1966,16 +1964,17 @@ detect(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "driver alloc failed"); - ret = g_dev->detect() + int ret = g_dev->detect(); delete g_dev; g_dev = nullptr; - if (ret) + if (ret) { /* nonzero, error */ exit(1); - else + } else { exit(0); + } } void -- cgit v1.2.3 From fff1856377b687bc290f4a828363de0d416b401c Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:11:58 +0900 Subject: There was a bug in position_estimator_mc. Solved --- src/modules/position_estimator_mc/position_estimator_mc_main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd1329..363961819 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) local_pos_est.vz = x_z_aposteriori_k[1]; local_pos_est.timestamp = hrt_absolute_time(); if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + if(local_pos_est_pub > 0) + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + else + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); + //mavlink_log_info(mavlink_fd, buf); } } } /* end of poll return value check */ -- cgit v1.2.3 From 9f2419698ff4d0703f58e67f29f4981fa7b4f0c5 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:22:19 +0900 Subject: Topic has been changed for quaternion-based attitude controller. --- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a7cda34a8..686fd765c 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + //! For quaternion-based attitude control + float q_d[4]; /** Desired quaternion for quaternion control */ + bool q_d_valid; /**< Set to true if quaternion vector is valid */ + float q_e[4]; /** Attitude error in quaternion */ + bool q_e_valid; /**< Set to true if quaternion error vector is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ }; -- cgit v1.2.3 From 6a6fe6937146723cb86dc0fc6e46db38b328dacd Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Sat, 31 Aug 2013 03:23:03 +0900 Subject: Bug in so3 estimator related to R matrix --- .../attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 236052b56..86bda3c75 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); - // Convert q->R. + // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 @@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { -- cgit v1.2.3 From efca6f2cb1848523969c7f29abba7389e48affbc Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 30 Aug 2013 20:12:35 -0400 Subject: Remove unused tones and save about 6K flash space - Comment out unused tones - Create symbolic tone numbers rather than hardcoded --- src/drivers/drv_tone_alarm.h | 20 ++ src/drivers/stm32/tone_alarm/tone_alarm.cpp | 321 ++++++++++++----------- src/modules/commander/commander_helper.cpp | 16 +- src/systemcmds/preflight_check/preflight_check.c | 6 +- src/systemcmds/tests/test_hrt.c | 4 +- 5 files changed, 194 insertions(+), 173 deletions(-) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 0c6afc64c..4a399076d 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -125,4 +125,24 @@ enum tone_pitch { TONE_NOTE_MAX }; +enum { + TONE_STOP_TUNE = 0, + TONE_STARTUP_TUNE, + TONE_ERROR_TUNE, + TONE_NOTIFY_POSITIVE_TUNE, + TONE_NOTIFY_NEUTRAL_TUNE, + TONE_NOTIFY_NEGATIVE_TUNE, + TONE_CHARGE_TUNE, + /* Do not include these unused tunes + TONE_DIXIE_TUNE, + TONE_CUCURACHA_TUNE, + TONE_YANKEE_TUNE, + TONE_DAISY_TUNE, + TONE_WILLIAM_TELL_TUNE, */ + TONE_ARMING_WARNING_TUNE, + TONE_BATTERY_WARNING_SLOW_TUNE, + TONE_BATTERY_WARNING_FAST_TUNE, + TONE_NUMBER_OF_TUNES +}; + #endif /* DRV_TONE_ALARM_H_ */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ad21f7143..a726247d5 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -233,8 +233,7 @@ public: private: static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * const _default_tunes[]; - static const unsigned _default_ntunes; + static const char * _default_tunes[TONE_NUMBER_OF_TUNES - 1]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -305,161 +304,7 @@ private: }; // predefined tune array -const char * const ToneAlarm::_default_tunes[] = { - "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune - "MBT200a8a8a8PaaaP", // ERROR tone - "MFT200e8a8a", // NotifyPositive tone - "MFT200e8e", // NotifyNeutral tone - "MFT200e8c8e8c8e8c8", // NotifyNegative tone - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! - "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie - "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha - "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64", - "MNT75L1O2G", //arming warning - "MBNT100a8", //battery warning slow - "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" //battery warning fast // XXX why is there a break before a repetition -}; - -const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); +const char * ToneAlarm::_default_tunes[TONE_NUMBER_OF_TUNES - 1]; // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -479,6 +324,162 @@ ToneAlarm::ToneAlarm() : { // enable debug() calls //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE - 1] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE - 1] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE - 1] = "MFT200e8a8a"; // NotifyPositive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE - 1] = "MFT200e8e"; // NotifyNeutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE - 1] = "MFT200e8c8e8c8e8c8"; // NotifyNegative tone + _default_tunes[TONE_CHARGE_TUNE - 1] = + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! +#if 0 // don't include unused tunes... but keep them for nostalgic reason + _default_tunes[TONE_DIXIE_TUNE - 1] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie + _default_tunes[TONE_CUCURACHA_TUNE - 1] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha + _default_tunes[TONE_YANKEE_TUNE - 1] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee + _default_tunes[TONE_DAISY_TUNE - 1] = + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy + _default_tunes[TONE_WILLIAM_TELL_TUNE - 1] = + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64"; +#endif + _default_tunes[TONE_ARMING_WARNING_TUNE - 1] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE - 1] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE - 1] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast } ToneAlarm::~ToneAlarm() @@ -873,7 +874,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= _default_ntunes) { + if (arg <= TONE_NUMBER_OF_TUNES) { if (arg == 0) { // stop the tune _tune = nullptr; @@ -1008,10 +1009,10 @@ tone_alarm_main(int argc, char *argv[]) } if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(1); + play_tune(TONE_STARTUP_TUNE); if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(0); + play_tune(TONE_STOP_TUNE); if ((tune = strtol(argv[1], nullptr, 10)) != 0) play_tune(tune); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 5df5d8d0c..7feace2b4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -99,44 +99,44 @@ void buzzer_deinit() void tune_error() { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } void tune_positive() { - ioctl(buzzer, TONE_SET_ALARM, 3); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { - ioctl(buzzer, TONE_SET_ALARM, 5); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() { - return ioctl(buzzer, TONE_SET_ALARM, 12); + return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); } int tune_low_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 13); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); } int tune_critical_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 14); + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } void tune_stop() { - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } static int leds; diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4c19dcffb..a323261cc 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -176,15 +176,15 @@ system_eval: led_toggle(leds, LED_AMBER); if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } usleep(100000); } /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); /* switch on leds */ led_on(leds, LED_BLUE); diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index f6e540401..ba6b86adb 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -137,7 +137,7 @@ int test_tone(int argc, char *argv[]) tone = atoi(argv[1]); if (tone == 0) { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); @@ -148,7 +148,7 @@ int test_tone(int argc, char *argv[]) } } else { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); -- cgit v1.2.3 From f246b68c7b53c44ae00e0f3fca724d1fbfab8f5d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 30 Aug 2013 20:24:14 -0400 Subject: Fix parameter range check --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a726247d5..262fe6e4c 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -874,7 +874,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= TONE_NUMBER_OF_TUNES) { + if (arg < TONE_NUMBER_OF_TUNES) { if (arg == 0) { // stop the tune _tune = nullptr; -- cgit v1.2.3 From b80f0d46ae97aaa4038958ff97165e0c154b745c Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 31 Aug 2013 01:03:32 -0400 Subject: Allow tone_alarm cmd to use tone names as well as number - remove script dependency on hard coded tone numbers - fix bug restarting repeating tone after stop --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- src/drivers/drv_tone_alarm.h | 2 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 86 ++++++++++++++++++----------- 4 files changed, 56 insertions(+), 36 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 85f00e582..aaf91b316 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -19,5 +19,5 @@ then fi else # SOS - tone_alarm 6 + tone_alarm error fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 982e638fb..b1648daa7 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -20,7 +20,7 @@ then else echo "[init] no microSD card found" # Play SOS - tone_alarm 2 + tone_alarm error fi # diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 4a399076d..f0b860620 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -132,8 +132,8 @@ enum { TONE_NOTIFY_POSITIVE_TUNE, TONE_NOTIFY_NEUTRAL_TUNE, TONE_NOTIFY_NEGATIVE_TUNE, - TONE_CHARGE_TUNE, /* Do not include these unused tunes + TONE_CHARGE_TUNE, TONE_DIXIE_TUNE, TONE_CUCURACHA_TUNE, TONE_YANKEE_TUNE, diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 262fe6e4c..d18106541 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -230,10 +230,14 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + inline const char *name(int tune) { + return _tune_names[tune]; + } private: static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * _default_tunes[TONE_NUMBER_OF_TUNES - 1]; + const char * _default_tunes[TONE_NUMBER_OF_TUNES]; + const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -303,9 +307,6 @@ private: }; -// predefined tune array -const char * ToneAlarm::_default_tunes[TONE_NUMBER_OF_TUNES - 1]; - // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -324,20 +325,20 @@ ToneAlarm::ToneAlarm() : { // enable debug() calls //_debug_enabled = true; - _default_tunes[TONE_STARTUP_TUNE - 1] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune - _default_tunes[TONE_ERROR_TUNE - 1] = "MBT200a8a8a8PaaaP"; // ERROR tone - _default_tunes[TONE_NOTIFY_POSITIVE_TUNE - 1] = "MFT200e8a8a"; // NotifyPositive tone - _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE - 1] = "MFT200e8e"; // NotifyNeutral tone - _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE - 1] = "MFT200e8c8e8c8e8c8"; // NotifyNegative tone - _default_tunes[TONE_CHARGE_TUNE - 1] = - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone #if 0 // don't include unused tunes... but keep them for nostalgic reason - _default_tunes[TONE_DIXIE_TUNE - 1] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie - _default_tunes[TONE_CUCURACHA_TUNE - 1] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha - _default_tunes[TONE_YANKEE_TUNE - 1] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee - _default_tunes[TONE_DAISY_TUNE - 1] = + _default_tunes[TONE_CHARGE_TUNE] = + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! + _default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie + _default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha + _default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee + _default_tunes[TONE_DAISY_TUNE] = "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy - _default_tunes[TONE_WILLIAM_TELL_TUNE - 1] = + _default_tunes[TONE_WILLIAM_TELL_TUNE] = "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" @@ -477,9 +478,18 @@ ToneAlarm::ToneAlarm() : "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64"; #endif - _default_tunes[TONE_ARMING_WARNING_TUNE - 1] = "MNT75L1O2G"; //arming warning - _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE - 1] = "MBNT100a8"; //battery warning slow - _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE - 1] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + + _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune + _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone + _tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive"; // Notify Positive tone + _tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral"; // Notify Neutral tone + _tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative"; // Notify Negative tone + _tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning + _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow + _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast } ToneAlarm::~ToneAlarm() @@ -875,16 +885,18 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) debug("TONE_SET_ALARM %u", arg); if (arg < TONE_NUMBER_OF_TUNES) { - if (arg == 0) { + if (arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; + _repeat = false; + _default_tune_number = 0; } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { /* play the selected tune */ _default_tune_number = arg; - start_tune(_default_tunes[arg - 1]); + start_tune(_default_tunes[arg]); } } } else { @@ -1008,22 +1020,30 @@ tone_alarm_main(int argc, char *argv[]) } } - if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(TONE_STARTUP_TUNE); + if (argc > 1) { + if (!strcmp(argv[1], "start")) + play_tune(TONE_STARTUP_TUNE); + + if (!strcmp(argv[1], "stop")) + play_tune(TONE_STOP_TUNE); - if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(TONE_STOP_TUNE); + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); - if ((tune = strtol(argv[1], nullptr, 10)) != 0) - play_tune(tune); + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv[1])) + play_tune(tune); - /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } } + } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); + errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name"); } -- cgit v1.2.3 From 7292e8c7220eb473fef5eb3a95a11fefd1d1e7e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:21:57 +0200 Subject: Hotfix for mavlink logbuffer, needs another round of validation. --- src/modules/mavlink/mavlink.c | 5 ++++- src/modules/systemlib/mavlink_log.c | 8 ++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d7b0fa9c7..78e01cefb 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&lb, 2); int ch; char *device_name = "/dev/ttyS1"; @@ -738,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); + /* destroy log buffer */ + mavlink_logbuffer_destroy(&lb); + thread_running = false; exit(0); diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 27608bdbf..03ca71375 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -54,6 +54,14 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); } +__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) +{ + lb->size = 0; + lb->start = 0; + lb->count = 0; + free(lb->elems); +} + __EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; -- cgit v1.2.3 From acc06e7eb1dafa8b4bc22b34c2b2c567d67e6b37 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:22:15 +0200 Subject: Added logbuffer free function --- src/include/mavlink/mavlink_log.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index a28ff3a68..5054937e0 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -107,7 +107,6 @@ struct mavlink_logmessage { struct mavlink_logbuffer { unsigned int start; - // unsigned int end; unsigned int size; int count; struct mavlink_logmessage *elems; @@ -115,6 +114,8 @@ struct mavlink_logbuffer { void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); +void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); + int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); -- cgit v1.2.3 From 669a8029212a715118bf380a524fb7ced64ccacc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:22:41 +0200 Subject: Hotfix: Better PX4IO detection feedbak --- src/drivers/px4io/px4io.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 47baa5770..c88abe59a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -497,7 +497,13 @@ PX4IO::detect() /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); if (protocol != PX4IO_PROTOCOL_VERSION) { - log("IO not installed"); + if (protocol == _io_reg_get_error) { + log("IO not installed"); + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + return -1; } log("IO found"); @@ -914,7 +920,6 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) int PX4IO::set_idle_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; if (len > _max_actuators) /* fail with error */ @@ -1612,7 +1617,7 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); @@ -2233,7 +2238,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 1500. */ uint16_t failsafe[8]; - for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { + for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { /* set channel to commandline argument or to 900 for non-provided channels */ if (argc > i + 2) { @@ -2265,7 +2270,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 900. */ uint16_t min[8]; - for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) { /* set channel to commanline argument or to 900 for non-provided channels */ if (argc > i + 2) { @@ -2300,7 +2305,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 2100. */ uint16_t max[8]; - for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) { /* set channel to commanline argument or to 2100 for non-provided channels */ if (argc > i + 2) { @@ -2335,7 +2340,7 @@ px4io_main(int argc, char *argv[]) /* set values for first 8 channels, fill unassigned channels with 0. */ uint16_t idle[8]; - for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) { /* set channel to commanline argument or to 0 for non-provided channels */ if (argc > i + 2) { -- cgit v1.2.3 From 56975e0dd1e68ed34194b86eb5f82c51ae648391 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:23:03 +0200 Subject: Hotfixed position control param update --- .../multirotor_pos_control.c | 61 +++++++++++----------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 8227f76c5..a25448af2 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -252,36 +252,35 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - int paramcheck_counter = 0; - while (!thread_should_exit) { - /* check parameters at 1 Hz */ - if (++paramcheck_counter >= 50) { - paramcheck_counter = 0; - bool param_updated; - orb_check(param_sub, ¶m_updated); - if (param_updated) { - parameters_update(¶ms_h, ¶ms); + bool param_updated; + orb_check(param_sub, ¶m_updated); - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; + if (param_updated) { + /* clear updated flag */ + struct parameter_update_s ps; + orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ + parameters_update(¶ms_h, ¶ms); - if (params.xy_vel_i == 0.0) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0; + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; - } else { - i_limit = 1.0f; // not used really - } + if (params.xy_vel_i == 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); + } else { + i_limit = 1.0f; // not used really } - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } + + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } bool updated; @@ -351,7 +350,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -377,7 +376,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ @@ -429,7 +428,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -444,7 +443,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (global_pos_sp_reproject) { @@ -468,7 +467,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (!local_pos_sp_valid) { @@ -481,7 +480,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; } - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } @@ -505,7 +504,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +514,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); } } @@ -527,7 +526,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } } @@ -588,7 +587,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); -- cgit v1.2.3 From 14cfb53534265cae8690734f2b0d98b8bbf34da5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:23:24 +0200 Subject: Hotfix: Correct frame print string --- ROMFS/px4fmu_common/init.d/11_dji_f450 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index fb141e8a7..1827a2c11 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -1,6 +1,6 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" # # Load default params for this platform -- cgit v1.2.3 From fb4ca82b84c5c49a968cdaf7a9fa8098a9ed9e15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:33:57 +0200 Subject: Hotfix: Cleanup on startup scripts --- ROMFS/px4fmu_common/init.d/rc.sensors | 8 -------- ROMFS/px4fmu_common/init.d/rc.usb | 8 ++++++++ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 61bb09728..2828a108b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -7,14 +7,6 @@ # Start sensor drivers here. # -# -# Check for UORB -# -if uorb start -then - echo "uORB started" -fi - ms5611 start adc start diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index abeed0ca3..9264985d6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -8,6 +8,14 @@ echo "Starting MAVLink on this USB console" # Stop tone alarm tone_alarm stop +# +# Check for UORB +# +if uorb start +then + echo "uORB started" +fi + # Tell MAVLink that this link is "fast" if mavlink stop then -- cgit v1.2.3 From 87f7ebdd75ac11139f7b1ae568590ab1eabbd45c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:38:29 +0200 Subject: Hotfix: Rely on gyro calibration --- .../attitude_estimator_ekf_main.cpp | 26 ++++++++++++---------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 65abcde1e..a70a14fe4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } + // XXX disabling init for now + initialized = true; + + // gyro_offsets[0] += raw.gyro_rad_s[0]; + // gyro_offsets[1] += raw.gyro_rad_s[1]; + // gyro_offsets[2] += raw.gyro_rad_s[2]; + // offset_count++; + + // if (hrt_absolute_time() - start_time > 3000000LL) { + // initialized = true; + // gyro_offsets[0] /= offset_count; + // gyro_offsets[1] /= offset_count; + // gyro_offsets[2] /= offset_count; + // } } else { -- cgit v1.2.3 From 3f4315b4767ff221936e135b3252794a952f2b95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:49:36 +0200 Subject: Hotfix: Increase stack size for low prio commander task --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ddd86d03..45e0307e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -594,7 +594,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; /* low priority */ -- cgit v1.2.3 From ccc5bef3af2852172b18f33edaffb809a0a1ffcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:56:39 +0200 Subject: Hotfix for IO-less systems --- ROMFS/px4fmu_common/init.d/rcS | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b1648daa7..9bb8e4a49 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -115,14 +115,18 @@ then echo "PX4IO running, not upgrading" else echo "Attempting to upgrade PX4IO" - px4io update - if [ -d /fs/microsd ] + if px4io update then - echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log - fi + if [ -d /fs/microsd ] + then + echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log + fi - # Allow IO to safely kick back to app - usleep 200000 + # Allow IO to safely kick back to app + usleep 200000 + else + echo "No PX4IO to upgrade here" + fi fi # -- cgit v1.2.3 From 54644e4206ffb40764f8f00e5ec41d98f54104a1 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 31 Aug 2013 16:06:15 -0400 Subject: Allow tone_alarm cmd to take filename as parameter - Restore ability to play beloved William Tell Overture... from file --- misc/tones/charge.txt | 1 + misc/tones/cucuracha.txt | 1 + misc/tones/daisy.txt | 1 + misc/tones/dixie.txt | 1 + misc/tones/tell.txt | 1 + misc/tones/yankee.txt | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 193 +++++----------------------- 7 files changed, 40 insertions(+), 159 deletions(-) create mode 100644 misc/tones/charge.txt create mode 100644 misc/tones/cucuracha.txt create mode 100644 misc/tones/daisy.txt create mode 100644 misc/tones/dixie.txt create mode 100644 misc/tones/tell.txt create mode 100644 misc/tones/yankee.txt diff --git a/misc/tones/charge.txt b/misc/tones/charge.txt new file mode 100644 index 000000000..cf33c58b3 --- /dev/null +++ b/misc/tones/charge.txt @@ -0,0 +1 @@ +MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4 \ No newline at end of file diff --git a/misc/tones/cucuracha.txt b/misc/tones/cucuracha.txt new file mode 100644 index 000000000..dce328c03 --- /dev/null +++ b/misc/tones/cucuracha.txt @@ -0,0 +1 @@ +MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8 \ No newline at end of file diff --git a/misc/tones/daisy.txt b/misc/tones/daisy.txt new file mode 100644 index 000000000..e39410621 --- /dev/null +++ b/misc/tones/daisy.txt @@ -0,0 +1 @@ +MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8 \ No newline at end of file diff --git a/misc/tones/dixie.txt b/misc/tones/dixie.txt new file mode 100644 index 000000000..f3ddd19ed --- /dev/null +++ b/misc/tones/dixie.txt @@ -0,0 +1 @@ +MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16 \ No newline at end of file diff --git a/misc/tones/tell.txt b/misc/tones/tell.txt new file mode 100644 index 000000000..ea77d1a24 --- /dev/null +++ b/misc/tones/tell.txt @@ -0,0 +1 @@ +T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8 P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8 O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16 O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4 O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8 O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16 O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8 O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16 O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8 O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8 O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16 O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8 O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16 O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16 O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16 O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16 O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8 O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16 O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64 O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16 O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16 O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16 O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16 O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16 O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16 O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16 O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16 O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16 O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16 O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16 O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16 O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16 O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16 O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16 O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16 O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16 O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16 O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16 O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16 O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16 O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16 O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16 O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16 O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16 O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16 O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16 O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16 O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16 O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16 O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16 O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16 O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16 O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16 O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8 O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16 O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8 O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8 O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8 O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16 O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16 O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8 O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8 O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16 O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16 O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16 O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16 O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16 O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16 O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16 O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16 O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16 O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16 P16O4E16P16D+16P16C+16P16O3B16P16A+16P16 O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16 O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16 O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16 O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16 O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16 O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16 O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16 O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8 O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16 O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8 O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16 O4E16P16D+16P16C+16P16O3B16P16A+16P16A16 P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16 O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8 O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16 O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16 O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16 O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16 O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16 O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8 O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16 O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16 O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8 O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8 O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8 O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16 O2E2P64 \ No newline at end of file diff --git a/misc/tones/yankee.txt b/misc/tones/yankee.txt new file mode 100644 index 000000000..3677b39c1 --- /dev/null +++ b/misc/tones/yankee.txt @@ -0,0 +1 @@ +MNT150L8O2GGABGBADGGABL4GL8F+ \ No newline at end of file diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index d18106541..a582ece17 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -235,7 +235,7 @@ public: } private: - static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes const char * _default_tunes[TONE_NUMBER_OF_TUNES]; const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; @@ -330,154 +330,6 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone -#if 0 // don't include unused tunes... but keep them for nostalgic reason - _default_tunes[TONE_CHARGE_TUNE] = - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge! - _default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16"; // dixie - _default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8"; // cucuracha - _default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+"; // yankee - _default_tunes[TONE_DAISY_TUNE] = - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8"; // daisy - _default_tunes[TONE_WILLIAM_TELL_TUNE] = - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64"; -#endif _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast @@ -983,7 +835,7 @@ play_tune(unsigned tune) } int -play_string(const char *str) +play_string(const char *str, bool free_buffer) { int fd, ret; @@ -995,6 +847,9 @@ play_string(const char *str) ret = write(fd, str, strlen(str) + 1); close(fd); + if (free_buffer) + free((void *)str); + if (ret < 0) err(1, "play tune"); exit(0); @@ -1021,29 +876,49 @@ tone_alarm_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "start")) + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) play_tune(TONE_STARTUP_TUNE); - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv1, "stop")) play_tune(TONE_STOP_TUNE); - if ((tune = strtol(argv[1], nullptr, 10)) != 0) + if ((tune = strtol(argv1, nullptr, 10)) != 0) play_tune(tune); /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv[1])) + if (!strcmp(g_dev->name(tune), argv1)) play_tune(tune); + /* If it is a file name then load and play it as a string */ + if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + if (fd == nullptr) + errx(1, "couldn't open '%s'", argv1); + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) + errx(1, "not enough memory memory"); + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + play_string(buffer, true); + } + /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + play_string(argv1, false); } } } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name"); + errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); } -- cgit v1.2.3 From a1c4c0fd787401f8ed332fa974a88a74ae079383 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 00:09:59 +0200 Subject: Comments on scaling factors --- src/modules/sensors/sensor_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bd431c9eb..6c8e514b6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -159,6 +159,8 @@ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +/* PX4IOAR: 0.00838095238 */ +/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); -- cgit v1.2.3 From c3408332fd0e6d77661a26fade8662a8b9be9970 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:12 +0200 Subject: Added test to test unlink() --- src/systemcmds/tests/tests_file.c | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 6f75b9812..47f480758 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -76,9 +76,39 @@ test_file(int argc, char *argv[]) close(fd); + unlink("/fs/microsd/testfile"); + warnx("512KiB in %llu microseconds", end - start); perf_print_counter(wperf); perf_free(wperf); + warnx("running unlink test"); + + /* ensure that common commands do not run against file count limits */ + for (unsigned i = 0; i < 64; i++) { + + warnx("unlink iteration #%u", i); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file before unlink()"); + int ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file before unlink()"); + close(fd); + + ret = unlink("/fs/microsd/testfile"); + if (ret != OK) + errx(1, "failed unlinking test file"); + + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file after unlink()"); + ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file after unlink()"); + close(fd); + } + return 0; } -- cgit v1.2.3 From f6bf1c7bf2d83bfc70fb8c4b9f589b25bcccc5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:30 +0200 Subject: Closing files that should be closed --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45e0307e6..8e8226f09 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -319,6 +319,8 @@ void print_status() break; } + close(state_sub); + warnx("arming: %s", armed_str); } @@ -1764,5 +1766,7 @@ void *commander_low_prio_loop(void *arg) } + close(cmd_sub); + return 0; } -- cgit v1.2.3 From 9eff3170a3aa63d17fbaefd39619866fb745b237 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:30:04 +0200 Subject: More verbosity on RC check --- src/modules/systemlib/rc_check.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 9d47d8000..f3a2aee9e 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -112,13 +112,13 @@ int rc_calibration_check(void) { } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } -- cgit v1.2.3 From 2d83c6f825db02f04624467f3d0b492ee371c72a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 12:47:10 +0200 Subject: Closing all opened file descriptors, fixed param save issue, tests clean --- .../commander/accelerometer_calibration.cpp | 4 +- src/modules/commander/airspeed_calibration.cpp | 9 +++-- src/modules/commander/gyro_calibration.cpp | 20 ++++++---- src/modules/commander/mag_calibration.cpp | 44 ++++++---------------- src/modules/commander/rc_calibration.cpp | 3 ++ src/modules/systemlib/param/param.c | 11 +++++- src/modules/systemlib/rc_check.c | 7 +++- src/systemcmds/preflight_check/preflight_check.c | 7 ++++ 8 files changed, 57 insertions(+), 48 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ed6707f04..cfa7d9e8a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) + if (orient < 0) { + close(sensor_combined_sub); return ERROR; + } if (data_collected[orient]) { mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index e414e5f70..248eb4a66 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + close(diff_pres_sub); return ERROR; } } @@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + close(diff_pres_sub); return ERROR; } @@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(diff_pres_sub); return ERROR; } - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - + close(diff_pres_sub); return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + close(diff_pres_sub); return ERROR; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 33566d4e5..82a0349c9 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - const int calibration_count = 5000; + const unsigned calibration_count = 5000; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; - int calibration_counter = 0; + unsigned calibration_counter = 0; float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; /* set offsets to zero */ @@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd) gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); } else { poll_errcount++; @@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + close(sub_sensor_combined); return ERROR; } } @@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); + close(sub_sensor_combined); return ERROR; } - mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); - mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); unsigned rotations_count = 30; @@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) && (hrt_absolute_time() - start_time > 5 * 1e6)) { - mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); + close(sub_sensor_combined); return OK; } @@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ + close(sub_sensor_combined); return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + close(sub_sensor_combined); return ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e38616027..b0d4266be 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd) axis_index++; mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd) break; } - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { @@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd) y[calibration_counter] = mag.y; z[calibration_counter] = mag.z; - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - calibration_counter++; + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + } else { poll_errcount++; @@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd) if (poll_errcount > 1000) { mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + close(sub_mag); + free(x); + free(y); + free(z); return ERROR; } @@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; + mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); free(x); free(y); @@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(sub_mag); return ERROR; } @@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + close(sub_mag); return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + close(sub_mag); return ERROR; } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index fe87a3323..9fc1d6470 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -40,6 +40,7 @@ #include "commander_helper.h" #include +#include #include #include #include @@ -80,9 +81,11 @@ int do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim calibration done"); + close(sub_man); return OK; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 69a9bdf9b..24b52d1a9 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -505,8 +505,15 @@ param_get_default_file(void) int param_save_default(void) { + int result; + /* delete the file in case it exists */ - unlink(param_get_default_file()); + struct stat buffer; + if (stat(param_get_default_file(), &buffer) == 0) { + result = unlink(param_get_default_file()); + if (result != OK) + warnx("unlinking file %s failed.", param_get_default_file()); + } /* create the file */ int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); @@ -516,7 +523,7 @@ param_save_default(void) return -1; } - int result = param_export(fd, false); + result = param_export(fd, false); close(fd); if (result != 0) { diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index f3a2aee9e..60d6473b8 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -66,7 +66,7 @@ int rc_calibration_check(void) { // count++; // } - + int channel_fail_count = 0; for (int i = 0; i < RC_CHANNELS_MAX; i++) { /* should the channel be enabled? */ @@ -142,7 +142,12 @@ int rc_calibration_check(void) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } + + channel_fail_count += count; } + + return channel_fail_count; } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index a323261cc..f5bd01ce8 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -142,6 +142,10 @@ int preflight_check_main(int argc, char *argv[]) bool rc_ok = (OK == rc_calibration_check()); + /* warn */ + if (!rc_ok) + warnx("rc calibration test failed"); + /* require RC ok to keep system_ok */ system_ok &= rc_ok; @@ -156,6 +160,9 @@ system_eval: } else { fflush(stdout); + warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); + fflush(stderr); + int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); -- cgit v1.2.3 From f0a750714ae07976b295c0a857d85cbd57a09a72 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Sep 2013 14:12:48 +0200 Subject: pid.c: fixed bad merge of seatbelt_multirotor, should fix EASY/AUTO modes --- src/modules/systemlib/pid/pid.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f66..562f3ac04 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -168,8 +168,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,11 +186,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit != 0.0f) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; -- cgit v1.2.3 From ff23feb24e2b8a9ad0bfc8fb1556939df5512310 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 15:32:40 +0200 Subject: Hotfix: Chasing log buffer issue --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 78e01cefb..a8ca19d7a 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 2); + mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; -- cgit v1.2.3 From e553087d10e7187c12e5e0f28937585a732a46da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 1 Sep 2013 20:17:24 +0200 Subject: pid.limit != 0 fix --- src/modules/systemlib/pid/pid.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 562f3ac04..739503ed4 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,6 +51,8 @@ #include "pid.h" #include +#define SIGMA 0.000001f + __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min) { @@ -168,7 +170,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,7 +188,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (pid->limit != 0.0f) { + if (pid->limit > SIGMA) { if (output > pid->limit) { output = pid->limit; -- cgit v1.2.3 From fddaa49ed8fadb5f2574225f121d6d8aa8654406 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 1 Sep 2013 22:44:58 +0200 Subject: Make sure the unit tests aren't included as part of the standard firmware --- makefiles/config_px4fmu-v1_default.mk | 4 ++-- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index fff43c6b4..3e94c3534 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -94,8 +94,8 @@ MODULES += modules/sdlog2 # # Unit tests # -MODULES += modules/unit_test -MODULES += modules/commander/commander_tests +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests # # Library modules diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ab0da3559..bd73bde5e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -88,8 +88,8 @@ MODULES += modules/sdlog2 # # Unit tests # -MODULES += modules/unit_test -MODULES += modules/commander/commander_tests +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests # # Library modules -- cgit v1.2.3 From 589ae937ee7afe5903f98f4eea25a26a79aca8f1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Sep 2013 17:55:27 +0200 Subject: Allow mixer upload when PWM is on --- src/modules/px4iofirmware/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index deed25836..0edd91b24 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while outputs armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { return; } -- cgit v1.2.3 From 027d45acbfa06dc155f6a18d0288fb8230d05c9f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Mon, 2 Sep 2013 09:13:36 -0700 Subject: respect NAV_TAKEOFF_ALT instead of using hardcoded default takeoff value --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 5 +++-- src/modules/multirotor_pos_control/multirotor_pos_control_params.c | 2 ++ src/modules/multirotor_pos_control/multirotor_pos_control_params.h | 2 ++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a25448af2..c194f627d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - const float takeoff_alt_default = 10.0f; + float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -244,6 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -423,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.z = -params.takeoff_alt; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 9c1ef2edb..4d6511851 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -59,6 +59,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -84,6 +85,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { + param_get(h->takeoff_alt,&(p->takeoff_alt)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 3ec85a364..79bc9b72b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,6 +41,7 @@ #include struct multirotor_position_control_params { + float takeoff_alt; float thr_min; float thr_max; float z_p; @@ -63,6 +64,7 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { + param_t takeoff_alt; param_t thr_min; param_t thr_max; param_t z_p; -- cgit v1.2.3 From 193a52d8132ce03aa0de9547f77ca8aeb67220f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 2 Sep 2013 23:13:01 +0200 Subject: multirotor_pos_control: added takeoff gap (hardcoded 3m), fixed code style --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c194f627d..5260a0963 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -244,7 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - + for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -351,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ @@ -424,12 +424,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = -params.takeoff_alt; + local_pos_sp.z = - params.takeoff_alt - 3.0f; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { @@ -505,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_sp_z = true; @@ -515,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_z) { reset_sp_z = false; local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } } -- cgit v1.2.3 From 40e18948727b41843fc0766e520f92b8cc841296 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 2 Sep 2013 23:30:32 +0200 Subject: Added parameter NAV_TAKEOFF_GAP --- src/modules/commander/commander_params.c | 1 + src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- src/modules/multirotor_pos_control/multirotor_pos_control_params.c | 4 +++- src/modules/multirotor_pos_control/multirotor_pos_control_params.h | 6 ++++-- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f22dac0c1..40d0386d5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -46,6 +46,7 @@ #include PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5260a0963..336523072 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -424,7 +424,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - 3.0f; + local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; local_pos_sp.yaw = att.yaw; local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 4d6511851..bf9578ea3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -60,6 +60,7 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); @@ -85,7 +86,8 @@ int parameters_init(struct multirotor_position_control_param_handles *h) int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->takeoff_alt,&(p->takeoff_alt)); + param_get(h->takeoff_alt, &(p->takeoff_alt)); + param_get(h->takeoff_gap, &(p->takeoff_gap)); param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 79bc9b72b..fc658dadb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -41,7 +41,8 @@ #include struct multirotor_position_control_params { - float takeoff_alt; + float takeoff_alt; + float takeoff_gap; float thr_min; float thr_max; float z_p; @@ -64,7 +65,8 @@ struct multirotor_position_control_params { }; struct multirotor_position_control_param_handles { - param_t takeoff_alt; + param_t takeoff_alt; + param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; -- cgit v1.2.3 From 29c06e39431eb6e5112a2cefa9969d59fa2ff875 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:36:43 +0200 Subject: Set sane defaults for F330 --- ROMFS/px4fmu_common/init.d/10_dji_f330 | 40 ++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index fb141e8a7..743dce9ef 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -10,18 +10,34 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 + param set MC_ATTRATE_D 0.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 1.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 2 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.10 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 param save fi -- cgit v1.2.3 From 3c0c11aec3c73d579659d81f9b68efbf7425846d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:49:57 +0200 Subject: v4 compatibility --- Tools/px_uploader.py | 2 +- src/drivers/px4io/uploader.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d2ebf1698..cffc31679 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -158,7 +158,7 @@ class uploader(object): INFO_BL_REV = chr(1) # bootloader protocol revision BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol + BL_REV_MAX = 4 # maximum supported bootloader protocol INFO_BOARD_ID = chr(2) # board type INFO_BOARD_REV = chr(3) # board revision INFO_FLASH_SIZE = chr(4) # max firmware size in bytes diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 135202dd1..22387a3e2 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -69,7 +69,7 @@ private: PROTO_REBOOT = 0x30, INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 3, /**< supported bootloader protocol */ + BL_REV = 4, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ -- cgit v1.2.3 From 791e22f44245676a743af5d73dd159f92fb1c159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 07:50:27 +0200 Subject: MAVLink workaround --- src/modules/mavlink/mavlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a8ca19d7a..052ce7948 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,6 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ + usleep(1000); mavlink_logbuffer_init(&lb, 10); int ch; -- cgit v1.2.3 From 2457013bbba3e15e3fbfcc45f07428f006d56dcd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Sep 2013 08:17:22 +0200 Subject: Hotfix for USB: Starting MAVLink only on USB if connected. Needs rewrite of MAVLink and delay / retries for correct approach --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- src/modules/mavlink/mavlink.c | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9bb8e4a49..8c79a035a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -63,9 +63,8 @@ then if sercon then echo "USB connected" - else - # second attempt - sercon & + sleep 3 + mavlink start -d /dev/ttyACM0 -b 230400 fi # @@ -105,7 +104,7 @@ then fi # Try to get an USB console - nshterm /dev/ttyACM0 & + #nshterm /dev/ttyACM0 & # # Upgrade PX4IO firmware @@ -219,5 +218,6 @@ then gps start fi + # End of autostart fi diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 052ce7948..a8ca19d7a 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -516,7 +516,6 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - usleep(1000); mavlink_logbuffer_init(&lb, 10); int ch; -- cgit v1.2.3