// Acquire state is used to wait for the GPS to achieve lock.
void ent_acquiringS()
{
flags._.GPS_steering = 0 ;
flags._.pitch_feedback = 0 ;
flags._.altitude_hold_throttle = 0 ;
flags._.altitude_hold_pitch = 0 ;
// almost ready to turn the control on, save the trims and sensor offsets
#if (FIXED_TRIMPOINT != 1) // Do not alter trims from preset when they are fixed
#if(USE_NV_MEMORY == 1)
if(udb_skip_flags.skip_radio_trim == 0)
{
udb_servo_record_trims() ;
}
#else
udb_servo_record_trims() ;
#endif
#endif
dcm_calibrate() ;
waggle = WAGGLE_SIZE ;
throttleFiltered._.W1 = 0 ;
stateS = &acquiringS ;
standby_timer = STANDBY_PAUSE ;
#if ( LED_RED_MAG_CHECK == 0 )
LED_RED = LED_OFF ;
#endif
return ;
}
// Acquire state is used to wait for the GPS to achieve lock.
void ent_acquiringS()
{
flags._.GPS_steering = 0 ;
flags._.pitch_feedback = 0 ;
flags._.altitude_hold_throttle = 0 ;
flags._.altitude_hold_pitch = 0 ;
// almost ready to turn the control on, save the trims and sensor offsets
udb_servo_record_trims() ;
dcm_calibrate() ;
waggle = WAGGLE_SIZE ;
throttleFiltered._.W1 = 0 ;
stateS = &acquiringS ;
standby_timer = STANDBY_PAUSE ;
#if ( LED_RED_MAG_CHECK == 0 )
LED_RED = LED_OFF ;
#endif
return ;
}
void MAVLinkCommandLong(mavlink_message_t* handle_msg) // MAVLINK_MSG_ID_COMMAND_LONG
{
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(handle_msg, &packet);
DPRINT("MAVLINK_MSG_ID_COMMAND_LONG %u\r\n", handle_msg->msgid);
//if (mavlink_check_target(packet.target, packet.target_component) == false) break;
{
switch (packet.command)
{
case MAV_CMD_PREFLIGHT_CALIBRATION:
DPRINT("MAV_CMD_PREFLIGHT_CALIBRATION %u\r\n", packet.command);
if (packet.param1 == 1)
{
#if (USE_NV_MEMORY ==1)
udb_skip_flags.skip_imu_cal = 0;
#endif // (USE_NV_MEMORY == 1)
udb_a2d_record_offsets();
}
else if (packet.param4 == 1) //param4 = radio calibration
{
if (udb_flags._.radio_on == 1)
{
udb_servo_record_trims();
command_ack(packet.command, MAV_CMD_ACK_OK);
}
else
command_ack(packet.command, MAV_CMD_ACK_ERR_FAIL);
}
else
command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED);
break;
#if (USE_NV_MEMORY == 1)
case MAV_CMD_PREFLIGHT_STORAGE:
DPRINT("MAV_CMD_PREFLIGHT_STORAGE %u\r\n", packet.command);
if (packet.param1 == MAV_PFS_CMD_WRITE_ALL) // 1
{
if (packet.param2 == MAV_PFS_CMD_WRITE_ALL)
data_services_save_all(STORAGE_FLAG_STORE_CALIB | STORAGE_FLAG_STORE_WAYPOINTS, &preflight_storage_complete_callback);
else
data_services_save_all(STORAGE_FLAG_STORE_CALIB, &preflight_storage_complete_callback);
}
else if (packet.param1 == MAV_PFS_CMD_READ_ALL) // 0
{
if (packet.param2 == MAV_PFS_CMD_READ_ALL)
data_services_load_all(STORAGE_FLAG_STORE_CALIB | STORAGE_FLAG_STORE_WAYPOINTS, &preflight_storage_complete_callback);
else
data_services_load_all(STORAGE_FLAG_STORE_CALIB, &preflight_storage_complete_callback);
}
else
command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED);
break;
case MAV_CMD_PREFLIGHT_STORAGE_ADVANCED:
DPRINT("MAV_CMD_PREFLIGHT_STORAGE_ADVANCED %u\r\n", packet.command);
switch ((uint16_t)packet.param1)
{
case MAV_PFS_CMD_CLEAR_SPECIFIC:
storage_clear_area(packet.param2, &preflight_storage_complete_callback);
break;
case MAV_PFS_CMD_WRITE_SPECIFIC:
data_services_save_specific(packet.param2, &preflight_storage_complete_callback);
break;
case MAV_PFS_CMD_READ_SPECIFIC:
data_services_load_specific(packet.param2, &preflight_storage_complete_callback);
break;
default:
command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED);
break;
}
break;
#endif // (USE_NV_MEMORY == 1)
case 245: // MAV_CMD_PREFLIGHT_STORAGE:
switch ((uint16_t)packet.param1)
{
case 0: // Read
DPRINT("Read (ROM)\r\n");
config_load();
break;
case 1: // Write
DPRINT("Write (ROM)\r\n");
config_save();
break;
default:
DPRINT("245 packet.param1 %f packet.param2 %f\r\n", (double)packet.param1, (double)packet.param2);
break;
}
break;
case 246: // halt
DPRINT("Halt - packet.command %u\r\n", packet.command);
break;
case 22: // start
DPRINT("Start - packet.command %u\r\n", packet.command);
break;
case 252: // land
DPRINT("Land - packet.command %u\r\n", packet.command);
break;
default:
DPRINT("packet.command %u\r\n", packet.command);
command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED);
break;
}
}
}
