/*
handle a MISSION_WRITE_PARTIAL_LIST mavlink packet
*/
void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// start waypoint receiving
if ((unsigned)packet.start_index > mission.num_commands() ||
(unsigned)packet.end_index > mission.num_commands() ||
packet.end_index < packet.start_index) {
send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));
return;
}
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_timelast_request = 0;
waypoint_receiving = true;
waypoint_request_i = packet.start_index;
waypoint_request_last= packet.end_index;
}
/** Configure 16 bit timer to trigger an ISR every second
*
* Configure "measurement in progress toggle LED-signal"
*/
void timer1_init(const uint16_t timer1_value)
{
orig_timer1_count = timer1_value;
timer1_count = timer1_value;
/** Safeguard: We cannot handle 0 or 1 count measurements. */
if (orig_timer1_count <= 1) {
send_text_P(PSTR("Unsupported timer value <= 1"));
wdt_soft_reset();
}
/* Compare match value into output compare reg. A */
OCR1A = TIMER1_COMPARE_MATCH_VAL;
/* Configure and start timer */
TCCR1A =
BITF(TIMER1_COMA_MODE, COM1A, 0) |
BITF(TIMER1_COMA_MODE, COM1A, 1) |
BITF(TIMER1_WGM_MODE, WGM1, 0) |
BITF(TIMER1_WGM_MODE, WGM1, 1);
TCCR1B =
BITF(TIMER1_PRESCALER, CS1, 0) |
BITF(TIMER1_PRESCALER, CS1, 1) |
BITF(TIMER1_PRESCALER, CS1, 2) |
BITF(TIMER1_WGM_MODE, WGM1, 2) |
BITF(TIMER1_WGM_MODE, WGM1, 3);
/* output compare match A interrupt enable */
TIMSK1 |= _BV(OCIE1A);
}
void send_eeprom_params_in_sram(void)
{
const uint8_t length = pparam_sram.length;
if (length == 0xff || length > sizeof(pparam_sram.params)) {
send_text_P(PSTR_INVALID_EEPROM_DATA);
} else {
frame_start(FRAME_TYPE_PARAMS_FROM_EEPROM, length);
uart_putb((const void *)pparam_sram.params, length);
frame_end();
}
}
/** Firmware FSM event handler for finished measurement */
inline static
firmware_state_t firmware_handle_measurement_finished(const firmware_state_t pstate)
{
switch (pstate) {
case STP_MEASURING:
/* end measurement */
cli();
on_measurement_finished();
send_table(PACKET_VALUE_TABLE_DONE);
return STP_DONE;
break;
default:
send_text_P(PSTR("invalid state transition"));
wdt_soft_reset();
break;
}
}
/*
handle a MISSION_WRITE_PARTIAL_LIST mavlink packet
*/
void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
// start waypoint receiving
if ((unsigned)packet.start_index > mission.num_commands() ||
(unsigned)packet.end_index > mission.num_commands() ||
packet.end_index < packet.start_index) {
send_text_P(SEVERITY_LOW,PSTR("flight plan update rejected"));
return;
}
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_timelast_request = 0;
waypoint_receiving = true;
waypoint_request_i = packet.start_index;
waypoint_request_last= packet.end_index;
}
/** Firmware FSM event handler for pressed switch */
inline static
firmware_state_t firmware_handle_switch_pressed(const firmware_state_t pstate)
{
switch (pstate) {
case STP_READY:
params_copy_from_eeprom_to_sram();
const uint8_t length = pparam_sram.length;
if ((length == 0xff) || (length > sizeof(pparam_sram.params))) {
send_text_P(PSTR_INVALID_EEPROM_DATA);
send_state_P(PSTR_READY);
return STP_READY;
} else {
general_personality_start_measurement_sram();
send_state_P(PSTR_MEASURING);
return STP_MEASURING;
}
break;
default:
/* silently ignore the switch press in all other states */
/* send_text_P(PSTR("ignoring pressed switch")); */
return pstate;
break;
}
}
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if its for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, NULL);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
send_text_P(SEVERITY_LOW,PSTR("command received: "));
switch(packet.command) {
case MAV_CMD_PREFLIGHT_CALIBRATION:
{
if (is_equal(packet.param1,1.0f)) {
tracker.ins.init_gyro();
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
if (is_equal(packet.param3,1.0f)) {
tracker.init_barometer();
// zero the altitude difference on next baro update
tracker.nav_status.need_altitude_calibration = true;
}
if (is_equal(packet.param4,1.0f)) {
// Cant trim radio
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
} else if (is_equal(packet.param5,2.0f)) {
// accel trim
float trim_roll, trim_pitch;
if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
result = MAV_RESULT_ACCEPTED;
break;
}
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (is_equal(packet.param1,1.0f)) {
tracker.arm_servos();
result = MAV_RESULT_ACCEPTED;
} else if (is_zero(packet.param1)) {
tracker.disarm_servos();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
tracker.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_SERVO:
if (tracker.servo_test_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
// mavproxy/mavutil sends this when auto command is entered
case MAV_CMD_MISSION_START:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1,1.0f)) {
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
result = MAV_RESULT_ACCEPTED;
}
break;
}
default:
break;
}
mavlink_msg_command_ack_send(
chan,
packet.command,
result);
break;
}
// When mavproxy 'wp sethome'
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (packet.start_index == 0)
{
// New home at wp index 0. Ask for it
waypoint_receiving = true;
waypoint_request_i = 0;
waypoint_request_last = 0;
send_message(MSG_NEXT_WAYPOINT);
waypoint_receiving = true;
}
break;
}
// XXX receive a WP from GCS and store in EEPROM if it is HOME
case MAVLINK_MSG_ID_MISSION_ITEM:
{
// decode
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
struct Location tell_command = {};
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2f; // in as m converted to cm
tell_command.options = 0; // absolute altitude
break;
}
#ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
#ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{
tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
default:
result = MAV_MISSION_UNSUPPORTED_FRAME;
break;
}
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the HOME wp
if (packet.seq == 0) {
tracker.set_home(tell_command); // New home in EEPROM
send_text_P(SEVERITY_LOW,PSTR("new HOME received"));
waypoint_receiving = false;
}
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send(
chan,
msg->sysid,
msg->compid,
result);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
tracker.tracking_manual_control(packet);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracker.tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracker.tracking_update_pressure(packet);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t));
break;
}
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, tracker.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, tracker.gps);
break;
#endif
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
break;
} // end switch
} // end handle mavlink
/*
handle an incoming mission item
*/
void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
{
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
struct AP_Mission::Mission_Command cmd = {};
mavlink_msg_mission_item_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) {
return;
}
// convert mavlink packet to mission command
if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) {
result = MAV_MISSION_INVALID;
goto mission_ack;
}
if (packet.current == 2) {
// current = 2 is a flag to tell us this is a "guided mode"
// waypoint and not for the mission
handle_guided_request(cmd);
// verify we received the command
result = 0;
goto mission_ack;
}
if (packet.current == 3) {
//current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
handle_change_alt_request(cmd);
// verify we recevied the command
result = 0;
goto mission_ack;
}
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// check if this is the requested waypoint
if (packet.seq != waypoint_request_i) {
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_ack;
}
// if command index is within the existing list, replace the command
if (packet.seq < mission.num_commands()) {
if (mission.replace_cmd(packet.seq,cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if command is at the end of command list, add the command
} else if (packet.seq == mission.num_commands()) {
if (mission.add_cmd(cmd)) {
result = MAV_MISSION_ACCEPTED;
}else{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if beyond the end of the command list, return an error
} else {
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// update waypoint receiving state machine
waypoint_timelast_receive = hal.scheduler->millis();
waypoint_request_i++;
if (waypoint_request_i >= waypoint_request_last) {
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
MAV_MISSION_ACCEPTED);
send_text_P(SEVERITY_LOW,PSTR("flight plan received"));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
} else {
waypoint_timelast_request = hal.scheduler->millis();
// if we have enough space, then send the next WP immediately
if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) {
queued_waypoint_send();
} else {
send_message(MSG_NEXT_WAYPOINT);
}
}
return;
mission_ack:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result);
}
inline static
void main_event_loop(void)
{
/** Frame parser FSM state */
typedef enum {
STF_MAGIC,
STF_COMMAND,
STF_LENGTH,
STF_PARAM,
STF_CHECKSUM0,
STF_CHECKSUM1,
} frame_state_t;
frame_state_t fstate = STF_MAGIC;
/** Frame parser offset/index into magic/data */
uint8_t idx = 0;
/** Frame parser cached data for current frame */
uint8_t cmd = 0;
/** Frame parser cached data for current frame */
uint8_t len = 0;
/** The UART checksum initialization code in uart-comm.c runs
* "uart_recv_checksum_reset()", i.e. we do not need to run it here
* a second time for the initial state.
*/
/* Firmware FSM State */
firmware_state_t pstate = STP_READY;
/* Globally enable interrupts */
sei();
/* Firmware main event loop */
while (1) {
/* check for "measurement finished" event */
if (GF_ISSET(GF_MEASUREMENT_FINISHED)) {
pstate = firmware_handle_measurement_finished(pstate);
GF_CLEAR(GF_MEASUREMENT_FINISHED);
continue;
}
/* check whether a key event occured */
if (switch_trigger_measurement()) {
pstate = firmware_handle_switch_pressed(pstate);
continue;
}
/* check whether a byte has arrived via UART */
if (bit_is_set(UCSR0A, RXC0)) {
/* The loop condition from uart_getc() checking UCSR0A for RCX0
* has just been checked here, so we directly read UDR0 instead
* of calling uart_getc(). */
const uint8_t byte = (uint8_t) UDR0;
uart_recv_checksum_update(byte);
frame_state_t next_fstate = fstate;
switch (fstate) {
case STF_MAGIC:
if (byte == pgm_read_byte_near(&(magic_header[idx++]))) {
if (idx < 4) {
next_fstate = STF_MAGIC;
} else {
next_fstate = STF_COMMAND;
}
} else {
/* syncing, not an error */
goto restart;
}
break;
case STF_COMMAND:
cmd = byte;
next_fstate = STF_LENGTH;
break;
case STF_LENGTH:
len = byte;
if (pstate == STP_READY) {
/* We can only use the personality_param_sram buffer in the
* STP_READY state. By not writing to the buffer after
* transitioning from STP_READY, we keep the content of the
* buffer from the "start measurement" command for sending
* back later.
*/
pparam_sram.length = len;
}
if (len == 0) {
next_fstate = STF_CHECKSUM0;
} else if ((len >= personality_param_size) &&
(len < MAX_PARAM_LENGTH)) {
idx = 0;
next_fstate = STF_PARAM;
} else {
/* whoever sent us that wrongly sized data frame made an error */
/** \todo Find a way to report errors without resorting to
* sending free text. */
send_text_P(PSTR("param length mismatch"));
goto error_restart_nomsg;
}
break;
case STF_PARAM:
if (pstate == STP_READY) {
/* We can only use the personality_param_sram buffer in the
* STP_READY state. By not writing to the buffer after
* transitioning from STP_READY, we keep the content of the
* buffer from the "start measurement" command for sending
* back later.
*/
pparam_sram.params[idx] = byte;
}
idx++;
if (idx < len) {
next_fstate = STF_PARAM;
} else {
next_fstate = STF_CHECKSUM0;
}
break;
case STF_CHECKSUM0:
/* We do not need to explicitly store the checksum here. A
* checksum match will happen implicitly when the checksum
* accumulator turns 0 which the code in uart-comm.c will
* detect for us.
*/
next_fstate = STF_CHECKSUM1;
break;
case STF_CHECKSUM1:
/* We do not need to explicitly store the checksum here. A
* checksum match will happen implicitly when the checksum
* accumulator turns 0 which the code in uart-comm.c will
* detect for us.
*/
if (uart_recv_checksum_matches()) {
/* checksum successful */
pstate = firmware_handle_command(pstate, cmd);
goto restart;
} else {
/** \todo Find a way to report checksum failure without
* resorting to sending free text. */
send_text_P(PSTR("checksum fail"));
goto error_restart_nomsg;
}
break;
}
//uprintf("idx=%u", idx);
goto skip_errors;
error_restart_nomsg:
restart:
next_fstate = STF_MAGIC;
idx = 0;
uart_recv_checksum_reset();
skip_errors:
fstate = next_fstate;
continue;
} /* character received on UART */
} /* while (1) main event loop */
} /* void main_event_loop(void); */
/** Firmware FSM event handler for receiving a command packet from the host
*
* \param pstate current FSM state
* \param cmd the command we are to handle
* \return new state
*
* Implicit parameters via global variables:
* personality_param_sram[0..sizeof(personality_param_sram)-2] param+token data
* personality_param_sram[sizeof(personality_param_sram)-1] size of param+token data
*/
inline static
firmware_state_t firmware_handle_command(const firmware_state_t pstate,
const uint8_t cmd)
{
/* temp vars */
const frame_cmd_t c = (frame_cmd_t)cmd;
uprintf("EAT PACKET: %c", cmd);
switch (pstate) {
case STP_READY:
switch (c) {
case FRAME_CMD_PERSONALITY_INFO:
send_personality_info();
/* fall through */
case FRAME_CMD_ABORT:
case FRAME_CMD_INTERMEDIATE:
case FRAME_CMD_STATE:
send_state_P(PSTR_READY);
return STP_READY;
break;
case FRAME_CMD_PARAMS_TO_EEPROM:
/* The param length has already been checked by the frame parser */
send_state_P(PSTR("PARAMS_TO_EEPROM"));
eeprom_update_block(&pparam_sram, &pparam_eeprom,
sizeof(pparam_eeprom));
send_state_P(PSTR_READY);
return STP_READY;
break;
case FRAME_CMD_PARAMS_FROM_EEPROM:
params_copy_from_eeprom_to_sram();
send_eeprom_params_in_sram();
send_state_P(PSTR_READY);
return STP_READY;
break;
case FRAME_CMD_MEASURE:
/* The param length has already been checked by the frame parser */
general_personality_start_measurement_sram();
send_state_P(PSTR_MEASURING);
return STP_MEASURING;
break;
case FRAME_CMD_RESET:
send_state_P(PSTR_RESET);
wdt_soft_reset();
break;
}
break;
case STP_MEASURING:
switch (c) {
case FRAME_CMD_INTERMEDIATE:
/** The value table will be updated asynchronously from ISRs
* like ISR(ADC_vect) or ISR(TIMER1_foo), i.e. independent from
* this main loop. This will cause glitches in the intermediate
* values as the values in the table often consist of more than
* a single 8bit machine word. However, we have decided that
* for *intermediate* results, those glitches are acceptable.
*
* Keeping interrupts enabled has the additional advantage that
* the measurement continues during send_table(), so we need not
* concern ourselves with pausing the measurement timer, or with
* making sure we properly reset the hardware which triggered
* our ISR within the appropriate time range or anything
* similar.
*
* If you decide to bracket the send_table() call with a
* cli()/sei() pair, be aware that you need to solve the issue
* of resetting the hardware properly. For example, with the
* adc-int-mca personality, you would need to reset the peak
* hold capacitor on resume if an event has been detected by the
* analog circuit while we had interrupts disabled and thus
* ISR(ADC_vect) could not reset the peak hold capacitor.
*/
send_table(PACKET_VALUE_TABLE_INTERMEDIATE);
send_state_P(PSTR_MEASURING);
return STP_MEASURING;
break;
case FRAME_CMD_PERSONALITY_INFO:
send_personality_info();
/* fall through */
case FRAME_CMD_PARAMS_TO_EEPROM:
case FRAME_CMD_PARAMS_FROM_EEPROM:
case FRAME_CMD_MEASURE:
case FRAME_CMD_RESET:
case FRAME_CMD_STATE:
send_state_P(PSTR_MEASURING);
return STP_MEASURING;
break;
case FRAME_CMD_ABORT:
send_state_P(PSTR_DONE);
cli();
on_measurement_finished();
send_table(PACKET_VALUE_TABLE_ABORTED);
send_state_P(PSTR_DONE);
return STP_DONE;
break;
}
break;
case STP_DONE:
switch (c) {
case FRAME_CMD_PERSONALITY_INFO:
send_personality_info();
/* fall through */
case FRAME_CMD_STATE:
send_state_P(PSTR_DONE);
return STP_DONE;
break;
case FRAME_CMD_RESET:
send_state_P(PSTR_RESET);
wdt_soft_reset();
break;
default:
send_table(PACKET_VALUE_TABLE_RESEND);
send_state_P(PSTR_DONE);
return STP_DONE;
break;
}
break;
}
send_text_P(PSTR("STP_ERROR"));
wdt_soft_reset();
}
