diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-13 18:53:37 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-13 18:53:37 +0200 |
commit | 9014577aff02233e890d1f8eefc06471fca8b6d2 (patch) | |
tree | bd589542e278bad3e4978d2098587ffc76c29681 /apps/commander/commander.c | |
parent | 56b3b46f75c0b434932eecba2ac7207f84e2342e (diff) | |
download | px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.tar.gz px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.tar.bz2 px4-firmware-9014577aff02233e890d1f8eefc06471fca8b6d2.zip |
Massive improvements in state machine, still tracing wrong throttle scaling in manual input path
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 94 |
1 files changed, 47 insertions, 47 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 68bb36b2d..19fc81f63 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -230,11 +230,11 @@ void cal_bsort(int16_t a[], int n) } } -void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) +void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { /* set to mag calibration mode */ - current_status->preflight_mag_calibration = true; - state_machine_publish(status_pub, current_status); + status->preflight_mag_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; @@ -312,8 +312,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) } /* disable calibration mode */ - current_status->preflight_mag_calibration = false; - state_machine_publish(status_pub, current_status); + status->preflight_mag_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); /* sort values */ cal_bsort(mag_minima[0], peak_samples); @@ -393,7 +393,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) free(mag_minima[2]); char offset_output[50]; - sprintf(offset_output, "[commander] mag calibration finished, offsets: x:%d, y:%d, z:%d", mag_offset[0], mag_offset[1], mag_offset[2]); + sprintf(offset_output, "[commander] mag cal: x:%8.4f y:%8.4f z:%8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); mavlink_log_info(mavlink_fd, offset_output); close(sub_sensor_combined); @@ -436,7 +436,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2]; char offset_output[50]; - sprintf(offset_output, "[commander] gyro calibration finished, offsets: x:%d, y:%d, z:%d", (int)gyro_offset[0], (int)gyro_offset[1], (int)gyro_offset[2]); + sprintf(offset_output, "[commander] gyro 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, offset_output); close(sub_sensor_combined); @@ -458,7 +458,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta switch (cmd->command) { case MAV_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1)) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_DENIED; @@ -466,18 +466,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; - case MAV_CMD_COMPONENT_ARM_DISARM: - { + case MAV_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_DENIED; @@ -487,19 +486,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta break; /* request for an autopilot reboot */ - case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - { - // if ((int)cmd->param1 == 1) { - // if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) { - // result = MAV_RESULT_ACCEPTED;//TODO: this has no effect - // } else { - // result = MAV_RESULT_DENIED; - // } - // } - + case MAV_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 = MAV_RESULT_ACCEPTED; + } else { + /* system may return here */ + result = MAV_RESULT_DENIED; + } + } } break; -// + // /* request to land */ // case MAV_CMD_NAV_LAND: // { @@ -524,18 +523,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // break; // /* preflight calibration */ - case MAV_CMD_PREFLIGHT_CALIBRATION: { + case MAV_CMD_PREFLIGHT_CALIBRATION: { bool handled = false; /* gyro calibration */ if ((int)(cmd->param1) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, SYSTEM_STATE_PREFLIGHT); + 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, "[commander] starting gyro calibration"); do_gyro_calibration(status_pub, ¤t_status); - do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration"); @@ -547,12 +546,12 @@ 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, SYSTEM_STATE_PREFLIGHT); + 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, "[commander] starting mag calibration"); do_mag_calibration(status_pub, ¤t_status); - do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration"); @@ -571,7 +570,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta break; /* preflight parameter load / store */ - case MAV_CMD_PREFLIGHT_STORAGE: { + case MAV_CMD_PREFLIGHT_STORAGE: { /* Read all parameters from EEPROM to RAM */ if (((int)cmd->param1) == 0) { @@ -610,7 +609,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; - default: { + default: { mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); result = MAV_RESULT_UNSUPPORTED; } @@ -625,8 +624,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* send acknowledge command */ mavlink_message_t msg; mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result); - //global_data_send_mavlink_message_out(&msg); - } } @@ -736,6 +733,8 @@ enum BAT_CHEM { * * @return the estimated remaining capacity in 0..1 */ +float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage); + float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage) { float ret = 0; @@ -783,17 +782,19 @@ int commander_main(int argc, char *argv[]) fprintf(stderr, "[commander] 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; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + state_machine_publish(stat_pub, ¤t_status, mavlink_fd); if (stat_pub < 0) { printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); exit(ERROR); } - /* make sure we are in preflight state */ - //do_state_update(stat_pub, ¤t_status, (commander_state_machine_t)SYSTEM_STATE_PREFLIGHT); - mavlink_log_info(mavlink_fd, "[commander] system is running"); /* load EEPROM parameters */ @@ -854,11 +855,10 @@ int commander_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); - uint8_t vehicle_state_previous = current_status.state_machine; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; uint64_t last_idle_time = 0; - unsigned int signal_loss_counter = 0; /* now initialized */ commander_initialized = true; @@ -891,7 +891,7 @@ int commander_main(int argc, char *argv[]) } /* toggle error led at 5 Hz in HIL mode */ - if ((current_status.mode & MAV_MODE_FLAG_HIL_ENABLED)) { + if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { /* armed */ led_toggle(LED_AMBER); @@ -994,7 +994,7 @@ int commander_main(int argc, char *argv[]) // // // if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests - update_state_machine_got_position_fix(stat_pub, ¤t_status); + update_state_machine_got_position_fix(stat_pub, ¤t_status, mavlink_fd); /* end: check gps */ /* Check battery voltage */ @@ -1017,7 +1017,7 @@ int commander_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, "[commander] EMERGENCY! CIRITICAL BATTERY!"); - state_machine_emergency(stat_pub, ¤t_status); + state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1044,7 +1044,7 @@ int commander_main(int argc, char *argv[]) /* check if left stick is in lower left position --> switch to standby state */ if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status); + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; } else { @@ -1056,7 +1056,7 @@ int commander_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status); + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; } else { @@ -1067,13 +1067,13 @@ int commander_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { - update_state_machine_mode_manual(stat_pub, ¤t_status); + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status); + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status); + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } /* Publish RC signal */ @@ -1112,11 +1112,11 @@ int commander_main(int argc, char *argv[]) current_status.preflight_gyro_calibration == false && current_status.preflight_mag_calibration == false) { /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, SYSTEM_STATE_STANDBY); + do_state_update(stat_pub, ¤t_status, SYSTEM_STATE_STANDBY, mavlink_fd); } /* Store old modes to detect and act on state transitions */ - vehicle_state_previous = current_status.state_machine; + // vehicle_state_previous = current_status.state_machine; voltage_previous = current_status.voltage_battery; fflush(stdout); |