static inline bool_t ins_configure(void)
{
// nothing to receive during conf
vn100_trans.input_length = 0;
switch (ins_init_status) {
case INS_VN100_SET_BAUD :
last_send_packet.RegID = VN100_REG_SBAUD;
vn100_trans.output_length = 4 + VN100_REG_SBAUD_SIZE;
ins_init_status++;
break;
case INS_VN100_SET_ADOR :
last_send_packet.RegID = VN100_REG_ADOR;
vn100_trans.output_length = 4 + VN100_REG_ADOR_SIZE;
ins_init_status++;
break;
case INS_VN100_SET_ADOF :
last_send_packet.RegID = VN100_REG_ADOF;
vn100_trans.output_length = 4 + VN100_REG_ADOF_SIZE;
ins_init_status++;
break;
case INS_VN100_READY :
return TRUE;
}
last_send_packet.CmdID = VN100_CmdID_WriteRegister;
spi_submit(&(VN100_SPI_DEV), &vn100_trans);
return FALSE;
}
void adxl345_write_to_reg(uint8_t _reg, uint8_t _val) {
imu_aspirin.accel_tx_buf[0] = _reg;
imu_aspirin.accel_tx_buf[1] = _val;
spi_submit(&(ADXL345_SPI_DEV), &aspirin_adxl345);
// FIXME: no busy waiting! if really needed add a timeout!!!!
while(aspirin_adxl345.status != SPITransSuccess);
}
static void mpu9250_spi_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
{
struct Mpu9250_Spi *mpu_spi = (struct Mpu9250_Spi *)(mpu);
mpu_spi->spi_trans.output_length = 2;
mpu_spi->spi_trans.input_length = 0;
mpu_spi->tx_buf[0] = _reg;
mpu_spi->tx_buf[1] = _val;
spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans));
}
void mpu_wait_slave4_ready(void)
{
while (!imu_aspirin2.slave4_ready) {
if (imu_aspirin2.wait_slave4_trans.status == SPITransDone) {
imu_aspirin2.wait_slave4_tx_buf[0] = MPU60X0_REG_I2C_MST_STATUS | MPU60X0_SPI_READ;
spi_submit(imu_aspirin2.mpu.spi_p, &(imu_aspirin2.wait_slave4_trans));
}
}
}
void mpu9250_spi_read(struct Mpu9250_Spi *mpu)
{
if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
mpu->spi_trans.output_length = 1;
mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes;
/* set read bit and multiple byte bit, then address */
mpu->tx_buf[0] = MPU9250_REG_INT_STATUS | MPU9250_SPI_READ;
spi_submit(mpu->spi_p, &(mpu->spi_trans));
}
}
void ms5611_spi_start_conversion(struct Ms5611_Spi *ms)
{
if (ms->status == MS5611_STATUS_IDLE &&
ms->spi_trans.status == SPITransDone) {
/* start D1 conversion */
ms->tx_buf[0] = MS5611_START_CONV_D1;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_CONV_D1;
}
}
void ms5611_spi_start_configure(struct Ms5611_Spi *ms)
{
if (ms->status == MS5611_STATUS_UNINIT) {
ms->initialized = FALSE;
ms->prom_cnt = 0;
ms->tx_buf[0] = MS5611_SOFT_RESET;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_RESET;
}
}
static inline void w5100_set(uint16_t _reg, uint8_t _val)
{
w5100_spi.output_buf[0] = 0xF0;
w5100_spi.output_buf[1] = _reg >> 8;
w5100_spi.output_buf[2] = _reg & 0xFF;
w5100_spi.output_buf[3] = _val;
spi_submit(&(W5100_SPI_DEV), &w5100_spi);
// FIXME: no busy waiting! if really needed add a timeout!!!!
while (w5100_spi.status != SPITransSuccess);
}
/**
* Periodic function to ensure proper delay after triggering reset or conversion.
* Should run at 100Hz max.
* Typical conversion time is 8.22ms at max resolution.
*/
void ms5611_spi_periodic_check(struct Ms5611_Spi *ms)
{
switch (ms->status) {
case MS5611_STATUS_RESET:
ms->status = MS5611_STATUS_RESET_OK;
break;
case MS5611_STATUS_RESET_OK:
if (ms->spi_trans.status == SPITransDone) {
/* start getting prom data */
ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_PROM;
}
break;
case MS5611_STATUS_CONV_D1:
ms->status = MS5611_STATUS_CONV_D1_OK;
break;
case MS5611_STATUS_CONV_D1_OK:
if (ms->spi_trans.status == SPITransDone) {
/* read D1 adc */
ms->tx_buf[0] = MS5611_ADC_READ;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_ADC_D1;
}
break;
case MS5611_STATUS_CONV_D2:
ms->status = MS5611_STATUS_CONV_D2_OK;
break;
case MS5611_STATUS_CONV_D2_OK:
if (ms->spi_trans.status == SPITransDone) {
/* read D2 adc */
ms->tx_buf[0] = MS5611_ADC_READ;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_ADC_D2;
}
break;
default:
break;
}
}
static inline uint8_t w5100_get(uint16_t _reg)
{
w5100_spi.output_buf[0] = 0x0F;
w5100_spi.output_buf[1] = _reg >> 8;
w5100_spi.output_buf[2] = _reg & 0xFF;
spi_submit(&(W5100_SPI_DEV), &w5100_spi);
// FIXME: no busy waiting! if really needed add a timeout!!!!
while (w5100_spi.status != SPITransSuccess);
return w5100_spi.input_buf[3];
}
/**
* Read multiple bytes from a register
*/
static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length)
{
if (cyrf->spi_t.status != SPITransDone) {
return FALSE;
}
/* Set the buffer and commit the transaction */
cyrf->spi_t.output_length = 1;
cyrf->spi_t.input_length = length + 1;
cyrf->output_buf[0] = addr;
// Submit the transaction
return spi_submit(cyrf->spi_p, &(cyrf->spi_t));
}
/** @brief Function sending a request to clear the writte enable flag in the memory
*
*/
void memory_send_wrdi(void)
{
memory_ready = FALSE;
msg[0] = 0x04;
memory_transaction.output_buf = (uint8_t *) msg;
memory_transaction.output_length = 1;
memory_transaction.input_buf = NULL;
memory_transaction.input_length = 0;
memory_transaction.after_cb = memory_transaction_done_cb;
spi_submit(&(HIGH_SPEED_LOGGER_DIRECT_MEMORY_DEVICE), &memory_transaction);
}
/** @brief Function sending a request to fetch the status Byte of the memory
*
*/
void memory_read_status_1(void)
{
memory_ready = FALSE;
msg[0] = 0x05;
memory_transaction.output_buf = (uint8_t *) msg;
memory_transaction.output_length = 1;
memory_transaction.input_buf = (uint8_t *) buff;
memory_transaction.input_length = 4;
memory_transaction.after_cb = memory_read_status_cb;
spi_submit(&(HIGH_SPEED_LOGGER_DIRECT_MEMORY_DEVICE), &memory_transaction);
}
/** @brief Function sending a request for the ID of the memory chip
*
* @todo change the cb function to actualy read the value returned
*/
void memory_read_id(void)
{
memory_ready = false;
msg[0] = 0x9F;
memory_transaction.output_buf = (uint8_t *) msg;
memory_transaction.output_length = 1;
memory_transaction.input_buf = (uint8_t *) buff;
memory_transaction.input_length = 24;
memory_transaction.after_cb = memory_transaction_done_cb;
spi_submit(&(HIGH_SPEED_LOGGER_DIRECT_MEMORY_DEVICE), &memory_transaction);
}
// Configuration function called once before normal use
void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu)
{
if (mpu->config.init_status == MPU9250_CONF_UNINIT) {
// First check if we found the chip (succesfull WHO_AM_I response)
if (mpu->spi_trans.status == SPITransSuccess && mpu->rx_buf[1] == MPU9250_WHOAMI_REPLY) {
mpu->config.init_status++;
mpu->spi_trans.status = SPITransDone;
mpu9250_send_config(mpu9250_spi_write_to_reg, (void *)mpu, &(mpu->config));
}
// Send WHO_AM_I to check if chip is there
else if (mpu->spi_trans.status != SPITransRunning && mpu->spi_trans.status != SPITransPending) {
mpu->spi_trans.output_length = 1;
mpu->spi_trans.input_length = 2;
mpu->tx_buf[0] = MPU9250_REG_WHO_AM_I | MPU9250_SPI_READ;
spi_submit(mpu->spi_p, &(mpu->spi_trans));
}
}
}
void ms2100_event(struct Ms2100 *ms) {
// handle request transaction
if (ms->req_trans.status == SPITransDone) {
if (ms->status == MS2100_GOT_EOC) {
// eoc occurs, submit reading req
spi_submit(ms->spi_p, &(ms->read_trans));
ms->status = MS2100_READING_RES;
}
}
else if (ms->req_trans.status == SPITransSuccess) {
ms->req_trans.status = SPITransDone;
}
else if (ms->req_trans.status == SPITransFailed) {
ms->status = MS2100_IDLE;
ms->cur_axe = 0;
ms->req_trans.status = SPITransDone;
}
// handle reading transaction
if (ms->read_trans.status == SPITransSuccess) {
if (ms->status == MS2100_READING_RES) {
// store value
int16_t new_val = Int16FromBuf(ms->read_buf,0);
// what is this check about?
if (abs(new_val) < 2000) {
ms->data.value[ms->cur_axe] = new_val;
}
ms->cur_axe++;
if (ms->cur_axe > 2) {
ms->cur_axe = 0;
ms->status = MS2100_DATA_AVAILABLE;
}
else {
ms->status = MS2100_IDLE;
}
ms->read_trans.status = SPITransDone;
}
}
else if (ms->read_trans.status == SPITransFailed) {
ms->status = MS2100_IDLE;
ms->cur_axe = 0;
ms->read_trans.status = SPITransDone;
}
}
void high_speed_logger_spi_link_periodic(void)
{
if (high_speed_logger_spi_link_ready) {
high_speed_logger_spi_link_ready = FALSE;
high_speed_logger_spi_link_data.gyro_p = imu.gyro_unscaled.p;
high_speed_logger_spi_link_data.gyro_q = imu.gyro_unscaled.q;
high_speed_logger_spi_link_data.gyro_r = imu.gyro_unscaled.r;
high_speed_logger_spi_link_data.acc_x = imu.accel_unscaled.x;
high_speed_logger_spi_link_data.acc_y = imu.accel_unscaled.y;
high_speed_logger_spi_link_data.acc_z = imu.accel_unscaled.z;
high_speed_logger_spi_link_data.mag_x = imu.mag_unscaled.x;
high_speed_logger_spi_link_data.mag_y = imu.mag_unscaled.y;
high_speed_logger_spi_link_data.mag_z = imu.mag_unscaled.z;
spi_submit(&(HIGH_SPEED_LOGGER_SPI_LINK_DEVICE), &high_speed_logger_spi_link_transaction);
}
high_speed_logger_spi_link_data.id++;
}
//在main.c中被main_periodic（）调用
void imu_periodic(void)
{
//imu_init()中imu_aspirin2的状态已设为Aspirin2StatusUninit
if (imu_aspirin2.status == Aspirin2StatusUninit) {
mpu_configure();
imu_aspirin2.status = Aspirin2StatusIdle;//imu空闲
aspirin2_mpu60x0.output_length = 22;//读和写的字数
aspirin2_mpu60x0.input_length = 22;
//mpu的读缓冲区的buf[0]为中断寄存器状态和spi读
aspirin2_mpu60x0.output_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
for (int i=1; i<aspirin2_mpu60x0.output_length; i++) {
aspirin2_mpu60x0.output_buf[i] = 0;//剩余的21个缓冲区存放为0
}
}
else {
spi_submit(&(MPU6000_SPI_DEV), &aspirin2_mpu60x0);
}
}
void vn100_periodic_task(void)
{
// only send config or request when last transaction is done
if (vn100_trans.status != SPITransDone) { return; }
// send request when configuration is done
if (ins_configure() == TRUE) {
// Fill request for QMR
last_send_packet.CmdID = VN100_CmdID_ReadRegister;
last_send_packet.RegID = VN100_REG_YMR;
// Set IO length
vn100_trans.output_length = 2; // Only 2 ?
vn100_trans.input_length = 4 + VN100_REG_YMR_SIZE;
// submit
spi_submit(&(VN100_SPI_DEV), &vn100_trans);
}
}
static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column)
{
uint8_t x = 0;
if (row > 15) {
column = 15;
}
if (column > 29) {
column = 29;
}
osd_char_address = ((uint16_t)row * 30) + column;
// translate the string and put it to the "osd_string" '\0' = 0xff
x = 0;
while (*(string + x) != '\0') {
osd_string[x] = ascii_to_osd_c(*(string + x));
x++;
}
osd_string[x] = ascii_to_osd_c(*(string + x));
for (x = 0; x < sizeof(osd_string); x++) {
if (osd_string[x] == 0xff) {
break;
}
}
//Adjust for the reserved character number.
for (; x < char_nb; x++) {
osd_string[x] = 0;
}
osd_string[x] = 0xff;
osd_attr = attributes;
//TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function.
if (max7456_osd_status == OSD_IDLE) {
max7456_trans.output_length = 2;
max7456_trans.output_buf[0] = OSD_DMAH_REG;
max7456_trans.output_buf[1] = (uint8_t)((osd_char_address >> 8) & 0x0001);
max7456_osd_status = OSD_S_STEP1;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
void imu_periodic(void) {
hmc5843_periodic();
if (imu_aspirin.status == AspirinStatusUninit) {
configure_gyro();
configure_accel();
//imu_aspirin_arch_int_enable();
imu_aspirin.accel_tx_buf[0] = (1<<7|1<<6|ADXL345_REG_DATA_X0);
imu_aspirin.status = AspirinStatusIdle;
} else {
imu_aspirin.gyro_available_blaaa = TRUE;
imu_aspirin.time_since_last_reading++;
imu_aspirin.time_since_last_accel_reading++;
spi_submit(&(ADXL345_SPI_DEV), &aspirin_adxl345);
//if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
// configure_accel();
// imu_aspirin.time_since_last_accel_reading=0;
//}
}
}
/**
* Write multiple bytes to a register
*/
static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[],
const uint8_t length)
{
uint8_t i;
/* Check if there is already a SPI transaction busy */
if (cyrf->spi_t.status != SPITransDone) {
return FALSE;
}
/* Set the buffer and commit the transaction */
cyrf->spi_t.output_length = length + 1;
cyrf->spi_t.input_length = 0;
cyrf->output_buf[0] = addr | CYRF_DIR;
// Copy the data
for (i = 0; i < length; i++) {
cyrf->output_buf[i + 1] = data[i];
}
// Submit the transaction
return spi_submit(cyrf->spi_p, &(cyrf->spi_t));
}
void ms5611_spi_event(struct Ms5611_Spi *ms)
{
if (ms->initialized) {
if (ms->spi_trans.status == SPITransFailed) {
ms->status = MS5611_STATUS_IDLE;
ms->spi_trans.status = SPITransDone;
} else if (ms->spi_trans.status == SPITransSuccess) {
// Successfull reading
switch (ms->status) {
case MS5611_STATUS_ADC_D1:
/* read D1 (pressure) */
ms->data.d1 = (ms->rx_buf[1] << 16) |
(ms->rx_buf[2] << 8) |
ms->rx_buf[3];
if (ms->data.d1 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
} else {
/* start D2 conversion */
ms->tx_buf[0] = MS5611_START_CONV_D2;
spi_submit(ms->spi_p, &(ms->spi_trans));
ms->status = MS5611_STATUS_CONV_D2;
}
break;
case MS5611_STATUS_ADC_D2:
/* read D2 (temperature) */
ms->data.d2 = (ms->rx_buf[1] << 16) |
(ms->rx_buf[2] << 8) |
ms->rx_buf[3];
if (ms->data.d2 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
} else {
/* calculate temp and pressure from measurements and set available if valid */
if (ms5611_calc(&(ms->data))) {
ms->data_available = TRUE;
}
ms->status = MS5611_STATUS_IDLE;
}
break;
default:
break;
}
ms->spi_trans.status = SPITransDone;
}
} else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
switch (ms->spi_trans.status) {
case SPITransFailed:
/* try again */
ms->status = MS5611_STATUS_UNINIT;
ms->spi_trans.status = SPITransDone;
break;
case SPITransSuccess:
if (ms->status == MS5611_STATUS_PROM) {
/* read prom data */
ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) |
ms->rx_buf[2];
if (ms->prom_cnt < PROM_NB) {
/* get next prom data */
ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
spi_submit(ms->spi_p, &(ms->spi_trans));
} else {
/* done reading prom, check prom crc */
if (ms5611_prom_crc_ok(ms->data.c)) {
ms->initialized = TRUE;
ms->status = MS5611_STATUS_IDLE;
} else {
/* checksum error, try again */
ms->status = MS5611_STATUS_UNINIT;
}
}
}
ms->spi_trans.status = SPITransDone;
break;
default:
break;
}
}
}
void link_mcu_send(void) {
ComputeChecksum(link_mcu_from_ap_msg);
link_mcu_from_ap_msg.checksum = crc;
spi_submit(&(LINK_MCU_SPI_DEV), &link_mcu_trans);
}
/// send request to read next axis
void ms2100_read(struct Ms2100 *ms) {
ms->req_buf[0] = (ms->cur_axe+1) << 0 | MS2100_DIVISOR << 4;
spi_submit(ms->spi_p, &(ms->req_trans));
ms->status = MS2100_SENDING_REQ;
}
void max7456_event(void) {
static uint8_t x = 0;
if (max7456_trans.status == SPITransSuccess) {
max7456_trans.status = SPITransDone;
switch (max7456_osd_status) {
case (OSD_INIT1):
max7456_osd_status = OSD_INIT2;
break;
case (OSD_INIT3):
max7456_trans.output_length = 2;
max7456_trans.input_length = 0;
max7456_trans.output_buf[0] = OSD_OSDBL_REG;
max7456_trans.output_buf[1] = max7456_trans.input_buf[0] & (~(1<<4));
max7456_osd_status = OSD_INIT4;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
break;
case (OSD_INIT4):
max7456_trans.output_buf[0] = OSD_VM0_REG;
#if USE_PAL_FOR_OSD_VIDEO
max7456_trans.output_buf[1] = OSD_VIDEO_MODE_PAL|osd_enable_val;
#else
max7456_trans.output_buf[1] = osd_enable_val;
#endif
max7456_osd_status = OSD_FINISHED;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
break;
case (OSD_READ_STATUS):
osd_stat_reg = max7456_trans.input_buf[0];
osd_stat_reg_valid = TRUE;
max7456_osd_status = OSD_FINISHED;
break;
case (OSD_S_STEP1):
max7456_trans.output_length = 2;
max7456_trans.output_buf[0] = OSD_DMAL_REG;
max7456_trans.output_buf[1] = (uint8_t)(osd_char_address);
max7456_osd_status = OSD_S_STEP2;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
break;
case (OSD_S_STEP2):
max7456_trans.output_length = 2;
max7456_trans.output_buf[0] = OSD_DMM_REG;
max7456_trans.output_buf[1] = OSD_AUTO_INCREMENT_MODE | osd_attr;
max7456_osd_status = OSD_S_STEP3;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
x = 0;
break;
case (OSD_S_STEP3):
max7456_trans.output_length = 1; //1 byte tranfers, auto address incrementing.
if (osd_string[x] != 0XFF) {
max7456_trans.output_buf[0] = osd_string[x++];
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else {
max7456_trans.output_buf[0] = 0xFF; //Exit the auto increment mode
max7456_osd_status = OSD_FINISHED;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
break;
case (OSD_FINISHED):
osd_attr = 0;
max7456_trans.status = SPITransDone;
max7456_osd_status = OSD_IDLE;
break;
default: break;
}
}
return;
}
void max7456_periodic(void) {
float temp = 0;
//This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
//If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
if (max7456_osd_status == OSD_IDLE) {
if(osd_enable > 1)
osd_enable = 1;
if ((osd_enable<<3) != osd_enable_val) {
osd_enable_val = (osd_enable<<3);
max7456_osd_status = OSD_UNINIT;
}
}
//INITIALIZATION OF THE OSD
if (max7456_osd_status == OSD_UNINIT) {
step = 0;
max7456_trans.status = SPITransDone;
max7456_trans.output_buf[0] = OSD_VM0_REG;
//This operation needs at least 100us but when the periodic function will be invoked again
//sufficient time will have elapsed even with at a periodic frequency of 1000 Hz
max7456_trans.output_buf[1] = OSD_RESET;
max7456_osd_status = OSD_INIT1;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else
if (max7456_osd_status == OSD_INIT2) {
max7456_trans.output_length = 1;
max7456_trans.input_length = 1;
max7456_trans.output_buf[0] = OSD_OSDBL_REG_R;
max7456_osd_status = OSD_INIT3;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else
if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN
//draw_osd();
switch (step) {
case (0):
osd_put_s("HDG", FALSE, 3, 0, 13);
step = 10;
break;
case (10):
temp = ((float)electrical.vsupply)/10;
osd_sprintf(osd_string, "%.1fV", temp );
if (temp > LOW_BAT_LEVEL)
osd_put_s(osd_string, FALSE, 8, 0, 2);
else
osd_put_s(osd_string, BLINK|INVERT, 8, 0, 2);
step = 20;
break;
case (20):
#if MAG_HEADING_AVAILABLE && !defined(SITL)
temp = DegOfRad(MAG_Heading);
if (temp < 0)
temp += 360;
#else
temp = DegOfRad(state.h_speed_dir_f);
if (temp < 0)
temp += 360;
#endif
osd_sprintf(osd_string, "%.0f", temp);
osd_put_s(osd_string, FALSE, 8, 1, 13);
step = 30;
break;
case (30):
osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f*3.6));
osd_put_s(osd_string, FALSE, 8, 0, 24);
step = 40;
break;
case (40):
osd_sprintf(osd_string, "%.0fm", GetPosAlt() );
osd_put_s(osd_string, FALSE, 10, 13, 2);
step = 50;
break;
case (50):
osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
osd_put_s(osd_string, FALSE, 7, 13, 24);
step = 10;
break;
default: break;
}
}
return;
}
