void AP_Mount_SToRM32_serial::parse_reply() {
uint16_t crc;
bool crc_ok;
switch (_reply_type) {
case ReplyType_DATA:
crc = crc_calculate(&_buffer[0], sizeof(_buffer.data) - 3);
crc_ok = crc == _buffer.data.crc;
if (!crc_ok) {
break;
}
_current_angle.x = _buffer.data.imu1_roll;
_current_angle.y = _buffer.data.imu1_pitch;
_current_angle.z = _buffer.data.imu1_yaw;
break;
case ReplyType_ACK:
crc = crc_calculate(&_buffer[1],
sizeof(SToRM32_reply_ack_struct) - 3);
crc_ok = crc == _buffer.ack.crc;
break;
default:
break;
}
}
void packet_assemble(packet_t* packet)
{
uint8_t* data_ptr = packet->hw_radio_packet.data + 1; // skip length field for now, we fill this later
data_ptr += dll_assemble_packet_header(packet, data_ptr);
data_ptr += d7anp_assemble_packet_header(packet, data_ptr);
data_ptr += d7atp_assemble_packet_header(packet, data_ptr);
// add payload
memcpy(data_ptr, packet->payload, packet->payload_length); data_ptr += packet->payload_length;
packet->hw_radio_packet.length = data_ptr - packet->hw_radio_packet.data - 1 + 2; // exclude the length byte and add CRC bytes
packet->hw_radio_packet.data[0] = packet->hw_radio_packet.length;
// TODO network protocol footer
// add CRC - SW CRC when using FEC
if (!has_hardware_crc || packet->hw_radio_packet.tx_meta.tx_cfg.channel_id.channel_header.ch_coding == PHY_CODING_FEC_PN9)
{
DPRINT_DATA_DLL(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1); // TODO tmp
uint16_t crc = __builtin_bswap16(crc_calculate(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1 - 2));
memcpy(data_ptr, &crc, 2);
DPRINT_DATA_DLL(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1); // TODO tmp
}
}
static void packet_received(hw_radio_packet_t* packet) {
uint16_t crc = __builtin_bswap16(crc_calculate(packet->data, packet->length - 2));
if(memcmp(&crc, packet->data + packet->length + 1 - 2, 2) != 0)
{
DPRINT("CRC error");
missed_packets_counter++;
}
else
{
#if HW_NUM_LEDS > 0
led_toggle(0);
#endif
uint16_t msg_counter = 0;
uint64_t msg_id;
memcpy(&msg_id, packet->data + 1, sizeof(msg_id));
memcpy(&msg_counter, packet->data + 1 + sizeof(msg_id), sizeof(msg_counter));
char chan[8];
channel_id_to_string(&(packet->rx_meta.rx_cfg.channel_id), chan, sizeof(chan));
console_printf("%7s,%i,%i,%lu,%lu,%i\n", chan, msg_counter, packet->rx_meta.rssi, (unsigned long)msg_id, (unsigned long)id, packet->rx_meta.timestamp);
if(counter == 0)
{
// just start, assume received all previous counters to reset PER to 0%
received_packets_counter = msg_counter - 1;
counter = msg_counter - 1;
}
uint16_t expected_counter = counter + 1;
if(msg_counter == expected_counter)
{
received_packets_counter++;
counter++;
}
else if(msg_counter > expected_counter)
{
missed_packets_counter += msg_counter - expected_counter;
counter = msg_counter;
}
else
{
sched_post_task(&start);
}
double per = 0;
if(msg_counter > 0)
per = 100.0 - ((double)received_packets_counter / (double)msg_counter) * 100.0;
sprintf(lcd_msg, "%i %i", (int)per, packet->rx_meta.rssi);
#ifdef PLATFORM_EFM32GG_STK3700
lcd_write_string(lcd_msg);
#else
lcd_write_line(4, lcd_msg);
#endif
}
}
//----------------------------------------------------------------------------------------------------------------------------------
// CRC check on IR-packets
// Param walk: pointer to incoming packet
// Param largecid: largecid value
// Param addcidUsed: value of addcidUsed, if 1 then addcid is used, else 0
// Param profile: pointer to profile of packet
// Return: 1 if CRC is OK, else 0
//----------------------------------------------------------------------------------------------------------------------------------
int rohc_ir_packet_crc_ok(unsigned char * walk, const int largecid, const int addcidUsed, const struct s_profile * profile)
{
int realcrc, crc;
realcrc = walk[largecid+2];
walk[largecid+2] = 0;
if(profile->id==0)
crc = crc_calculate(CRC_TYPE_8, walk-addcidUsed, profile->detect_ir_size(walk, largecid+1)+2+largecid+addcidUsed);
else
crc = crc_calculate(CRC_TYPE_8, walk-addcidUsed, profile->detect_ir_size(walk, largecid+1)+3+largecid+addcidUsed);
walk[largecid+2] = realcrc;
if(crc != realcrc) {
rohc_debugf(0,"ROHC Decompress IR: CRC FAILED! SKA: %i, �: %i\n", realcrc, crc);
return(0);
}
rohc_debugf(2,"ROHC Decompress IR: CRC OK!\n");
return(1);
}
static void transmit_packet() {
DPRINT("transmitting packet");
current_state = STATE_RUNNING;
counter++;
memcpy(data + 1, &id, sizeof(id));
memcpy(data + 1 + sizeof(id), &counter, sizeof(counter));
uint16_t crc = __builtin_bswap16(crc_calculate(data, sizeof(data) - 2));
memcpy(data + sizeof(data) - 2, &crc, 2);
memcpy(&tx_packet->data, data, sizeof(data));
tx_cfg.channel_id = current_channel_id;
tx_packet->tx_meta.tx_cfg = tx_cfg;
hw_radio_send_packet(tx_packet, &packet_transmitted);
}
// Check that the data storage table is valid with the correct checksum
boolean data_storage_check_table(void)
{
uint16_t mem_checksum;
// Check that the preamble is correct
if (memcmp(data_storage_table.table_preamble, table_storage_preamble, 4) != 0) return false;
// Calcualte the checksum
mem_checksum = crc_calculate((uint8_t*) data_storage_table.table, sizeof(*data_storage_table.table));
if (mem_checksum != data_storage_table.table_checksum) return false;
return true;
}
bool com_checksum(uint8_t *data, int32_t length) //Gets an array, length of its payload
//and checksums on the com-ports
{
if (length < 5)
return 0;
uint16_t res = crc_calculate(data, length + 6);
crc_accumulate(crcs[data[5]], &res);
uint8_t ck_a = (uint8_t)(res & 0xFF); //< High byte
uint8_t ck_b = (uint8_t)(res >> 8); //< Low byte
if ((ck_a == (uint8_t)data[length + 6]) && (ck_b == (uint8_t)data[length + 7])) //If checksums are equal to each other, then
return true; //return true
else
return false;
}
void packet_disassemble(packet_t* packet)
{
log_print_data(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1); // TODO tmp
uint16_t crc = __builtin_bswap16(crc_calculate(packet->hw_radio_packet.data, packet->hw_radio_packet.length - 2));
if(memcmp(&crc, packet->hw_radio_packet.data + packet->hw_radio_packet.length + 1 - 2, 2) != 0)
{
DPRINT(LOG_STACK_DLL, "CRC invalid");
goto cleanup;
}
uint8_t data_idx = 1;
if(!dll_disassemble_packet_header(packet, &data_idx))
{
DPRINT(LOG_STACK_DLL, "disassemble header failed");
goto cleanup;
}
// TODO assuming D7ANP for now
if(!d7anp_disassemble_packet_header(packet, &data_idx))
{
DPRINT(LOG_STACK_NWL, "disassemble header failed");
goto cleanup;
}
if(!d7atp_disassemble_packet_header(packet, &data_idx))
{
DPRINT(LOG_STACK_TRANS, "disassemble header failed");
goto cleanup;
}
// TODO footers
// extract payload
packet->payload_length = packet->hw_radio_packet.length + 1 - data_idx - 2; // exclude the headers CRC bytes // TODO exclude footers
memcpy(packet->payload, packet->hw_radio_packet.data + data_idx, packet->payload_length);
DPRINT(LOG_STACK_FWK, "Done disassembling packet");
d7atp_process_received_packet(packet);
return;
cleanup:
DPRINT(LOG_STACK_FWK, "Skipping packet");
packet_queue_free_packet(packet);
return;
}
void packet_disassemble(packet_t* packet)
{
DPRINT_DATA_DLL(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1); // TODO tmp
if (packet->hw_radio_packet.rx_meta.crc_status == HW_CRC_UNAVAILABLE)
{
uint16_t crc = __builtin_bswap16(crc_calculate(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1 - 2));
if(memcmp(&crc, packet->hw_radio_packet.data + packet->hw_radio_packet.length + 1 - 2, 2) != 0)
{
DPRINT_DLL("CRC invalid");
DPRINT_DATA_DLL(&crc, 2);
goto cleanup;
}
}
else if (packet->hw_radio_packet.rx_meta.crc_status == HW_CRC_INVALID)
{
DPRINT_DLL("CRC invalid");
goto cleanup;
}
uint8_t data_idx = 1;
if(!dll_disassemble_packet_header(packet, &data_idx))
goto cleanup;
// TODO assuming D7ANP for now
if(!d7anp_disassemble_packet_header(packet, &data_idx))
goto cleanup;
if(!d7atp_disassemble_packet_header(packet, &data_idx))
goto cleanup;
// TODO footers
// extract payload
packet->payload_length = packet->hw_radio_packet.length + 1 - data_idx - 2; // exclude the headers CRC bytes // TODO exclude footers
memcpy(packet->payload, packet->hw_radio_packet.data + data_idx, packet->payload_length);
DPRINT_FWK("Done disassembling packet");
d7atp_process_received_packet(packet);
return;
cleanup:
DPRINT_FWK("Skipping packet");
packet_queue_free_packet(packet);
return;
}
/**
* @brief Check whether the CRC on uncompressed header is correct or not
*
* @param context The decompression context
* @param uncomp_hdrs The uncompressed headers
* @param crc_pkt The CRC over uncompressed headers extracted from packet
* @return true if the CRC is correct, false otherwise
*/
bool rohc_decomp_check_uncomp_crc(const struct rohc_decomp_ctxt *const context,
struct rohc_buf *const uncomp_hdrs,
const struct rohc_decomp_crc_one *const crc_pkt)
{
uint8_t crc_computed;
/* determine the initial value and the pre-computed table for the CRC */
switch(crc_pkt->type)
{
case ROHC_CRC_TYPE_3:
crc_computed = CRC_INIT_3;
break;
case ROHC_CRC_TYPE_7:
crc_computed = CRC_INIT_7;
break;
case ROHC_CRC_TYPE_8:
rohc_decomp_warn(context, "unexpected CRC type %d", crc_pkt->type);
assert(0);
goto error;
case ROHC_CRC_TYPE_NONE:
default:
rohc_decomp_warn(context, "unknown CRC type %d", crc_pkt->type);
assert(0);
goto error;
}
/* compute the CRC from built uncompressed headers */
crc_computed =
crc_calculate(crc_pkt->type, rohc_buf_data(*uncomp_hdrs), uncomp_hdrs->len,
crc_computed);
rohc_decomp_debug(context, "CRC-%d on uncompressed header = 0x%x",
crc_pkt->type, crc_computed);
/* does the computed CRC match the one in packet? */
if(crc_computed != crc_pkt->bits)
{
rohc_decomp_warn(context, "CRC failure (computed = 0x%02x, packet = "
"0x%02x)", crc_computed, crc_pkt->bits);
goto error;
}
/* computed CRC matches the one in packet */
return true;
error:
return false;
}
// send_target_angles
void AP_Mount_SToRM32_serial::send_target_angles(float pitch_deg, float roll_deg, float yaw_deg)
{
static cmd_set_angles_struct cmd_set_angles_data = {
0xFA,
0x0E,
0x11,
0, // pitch
0, // roll
0, // yaw
0, // flags
0, // type
0, // crc
};
// exit immediately if not initialised
if (!_initialised) {
return;
}
if ((size_t)_port->txspace() < sizeof(cmd_set_angles_data)) {
return;
}
// reverse pitch and yaw control
pitch_deg = -pitch_deg;
yaw_deg = -yaw_deg;
// send CMD_SETANGLE
cmd_set_angles_data.pitch = pitch_deg;
cmd_set_angles_data.roll = roll_deg;
cmd_set_angles_data.yaw = yaw_deg;
uint8_t* buf = (uint8_t*)&cmd_set_angles_data;
cmd_set_angles_data.crc = crc_calculate(&buf[1], sizeof(cmd_set_angles_data)-3);
for (uint8_t i = 0; i != sizeof(cmd_set_angles_data) ; i++) {
_port->write(buf[i]);
}
// store time of send
_last_send = AP_HAL::millis();
}
void packet_assemble(packet_t* packet)
{
uint8_t* data_ptr = packet->hw_radio_packet.data + 1; // skip length field for now, we fill this later
data_ptr += dll_assemble_packet_header(packet, data_ptr);
data_ptr += d7anp_assemble_packet_header(packet, data_ptr);
data_ptr += d7atp_assemble_packet_header(packet, data_ptr);
// add payload
memcpy(data_ptr, packet->payload, packet->payload_length); data_ptr += packet->payload_length;
packet->hw_radio_packet.length = data_ptr - packet->hw_radio_packet.data - 1 + 2; // exclude the length byte and add CRC bytes
// TODO network protocol footer
// add CRC
uint16_t crc = __builtin_bswap16(crc_calculate(packet->hw_radio_packet.data, packet->hw_radio_packet.length - 2));
memcpy(data_ptr, &crc, 2);
}
static void wd1770_command_readaddress(BYTE command, unsigned int dnr)
{
drive_t *drive;
drive = drive_context[dnr]->drive;
#ifdef WD_DEBUG
log_debug("C:READ ADDRESS");
#endif
wd1770[dnr].type = 3;
wd1770[dnr].busy_clk = drive_clk[dnr];
wd1770_clear_errors(dnr);
if (drive->type == DRIVE_TYPE_1570
|| drive->type == DRIVE_TYPE_1571
|| drive->type == DRIVE_TYPE_1571CR) {
/* 1571 MFM disk images are not supported. */
wd1770[dnr].record_not_found = 1;
} else {
#if 0
WORD crc;
#endif
wd1770[dnr].data_buffer[0] = wd1770[dnr].track;
wd1770[dnr].data_buffer[1] = 0x0;
wd1770[dnr].data_buffer[2] = 0x1;
wd1770[dnr].data_buffer[3] = 0x2;
#if 0
crc = crc_calculate(&(wd1770[dnr].data_buffer[5]),
&(wd1770[dnr].data_buffer[2]));
wd1770[dnr].data_buffer[4] = crc >> 8;
wd1770[dnr].data_buffer[5] = crc & 255;
#endif
wd1770[dnr].data_buffer_index = 5;
wd1770[dnr].data_buffer_offset = 0;
wd1770_motor_control(command, dnr);
}
wd1770[dnr].wp_status = 0;
}
// Format the data storage table
boolean data_storage_format_table(void)
{
uint16_t mem_counter;
//Copy the preamble
memcpy(data_storage_table.table_preamble, table_storage_preamble, 4);
for(mem_counter = 0; mem_counter < MAX_DATA_HANDLES; mem_counter++)
{
data_storage_table.table[mem_counter].data_address = 0;
data_storage_table.table[mem_counter].data_type = DATA_STORAGE_NULL;
data_storage_table.table[mem_counter].data_size = 0;
}
// Calculate the checksum
mem_counter = crc_calculate((uint8_t*) data_storage_table.table, sizeof(*data_storage_table.table));
data_storage_table.table_checksum = mem_counter;
// Store the table
if (udb_nv_memory_write((uint8_t*) &data_storage_table, 0, sizeof(data_storage_table), &data_storage_format_callback) == false)
return false;
return true;
}
void RC_UART::loop()
{
union {
uint16_t period[NUM_CHANNELS];
uint8_t bytes[NUM_CHANNELS*2];
} u;
// wait for magic
while (true) {
uint8_t c = read_wait();
if (c == ESC_MAGIC) break;
// hal.console->printf("c=0x%02x\n", (unsigned)c);
}
uint8_t nbytes=0;
// wait for periods
while (nbytes < NUM_CHANNELS*2) {
u.bytes[nbytes++] = read_wait();
}
// and CRC
union {
uint8_t crc[2];
uint16_t crc16;
} u2;
u2.crc[0] = read_wait();
u2.crc[1] = read_wait();
uint16_t crc2 = crc_calculate(u.bytes, NUM_CHANNELS*2);
if (crc2 != u2.crc16) {
hal.console->printf("bad CRC 0x%04x should be 0x%04x\n", (unsigned)crc2, (unsigned)u2.crc16);
return;
}
// and output
for (uint8_t i=0; i<NUM_CHANNELS; i++) {
if (u.period[i] == 0) {
continue;
}
if (!(enable_mask & 1U<<i)) {
if (enable_mask == 0) {
hal.rcout->force_safety_off();
}
rc[i].enable_out();
enable_mask |= 1U<<i;
}
rc[i].set_radio_out(u.period[i]);
rc[i].output();
}
// report periods to console for debug
counter++;
if (counter % 100 == 0) {
hal.console->printf("%4u %4u %4u %4u\n",
(unsigned)u.period[0],
(unsigned)u.period[1],
(unsigned)u.period[2],
(unsigned)u.period[3]);
}
// every 10th frame give an RCInput frame if possible
if (counter % 10 == 0) {
struct PACKED {
uint8_t magic = 0xf6;
uint16_t rcin[8];
uint16_t crc;
} rcin;
if (hal.rcin->new_input() && hal.rcin->read(rcin.rcin, 8) == 8) {
rcin.crc = crc_calculate((uint8_t*)&rcin.rcin[0], 16);
hal.UART->write((uint8_t*)&rcin, sizeof(rcin));
}
}
}
void mavlink_parse(){
int payload_length = 0;
uint8_t *p_checksum;
uint16_t checksum, checksum_calc;
uint8_t msg_id;
bool payload_received;
pos_front = MAVLINK_BUFFER_SIZE - __HAL_DMA_GET_COUNTER((Uart3Handle.hdmarx));
//printf("%d\r\n", sizeof(mavlink_buffer));
while(1){
payload_received = false;
if( pos_front >= pos_back ){
distance = pos_front - pos_back;
}else{
distance = MAVLINK_BUFFER_SIZE + pos_front - pos_back;
}
if( distance >= 8 ){
//printf("distance: %d\r\n", distance);
payload_length = *(mavlink_buffer + (pos_back + P_LEN)%MAVLINK_BUFFER_SIZE );
msg_id = *(mavlink_buffer + (pos_back + P_MSG)%MAVLINK_BUFFER_SIZE );
if( 0xfe == *(mavlink_buffer + pos_back) && MAVLINK_MSG_ID_ATTITUDE == msg_id ){ // search sycn code and msg_id I want
//printf("sync found: %d\r\n", pos_back);
//printf("payload_length: %d\r\n", payload_length);
//printf("msg_id: %x\r\n", msg_id);
if( payload_length + 8 <= distance ){ // a full attitude payload has been received
//printf("length enough\r\n");
// copy the payload
if( pos_back + 6 + payload_length < MAVLINK_BUFFER_SIZE ){
memcpy( msg, mavlink_buffer + pos_back + 1, payload_length + 5 );
}else{
memcpy( msg, mavlink_buffer + pos_back + 1, MAVLINK_BUFFER_SIZE - (pos_back + 1) );
memcpy( msg + MAVLINK_BUFFER_SIZE - (pos_back + 1), mavlink_buffer, (payload_length + 5) - (MAVLINK_BUFFER_SIZE - (pos_back + 1)) );
}
p_checksum = (mavlink_buffer + (pos_back + 6 + payload_length) % MAVLINK_BUFFER_SIZE);
checksum = *p_checksum + 0x100 * ( *(p_checksum + 1) );
//printf("checksum: %x\r\n", checksum);
checksum_calc = crc_calculate( msg, payload_length + 5 );
crc_accumulate(39, &checksum_calc);
//printf("checksum: %x\r\n", checksum_calc);
// crc check
if ( checksum == checksum_calc){
//printf("checksum ok\r\n");
payload_received = true;
payload = (mavlink_attitude_t *)((uint8_t *)msg + 5);
real_roll = payload->roll;
real_pitch = payload->pitch;
}else{
payload_received = false;
}
}else{
//printf("length not enough\r\n");
return; // length is not enough, so return;
}
}else{
//printf("not found\r\n");
}
}else{
return;// length is not enough, so return;
}
if( payload_received ){
pos_back = ( pos_back + payload_length + 8 ) % MAVLINK_BUFFER_SIZE;
}else{
pos_back =( pos_back + 1 ) % MAVLINK_BUFFER_SIZE;
}
}
}
// Initialise the storage
// If read has failed keep re-trying until the nv memory is ready.
void data_storage_service(void)
{
switch (data_storage_status)
{
case DATA_STORAGE_STATUS_START:
data_storage_table.table_preamble[0] = 0x00; // Make sure memory contents are invalid before reading
data_storage_table.table_checksum = 0x0000;
// Loading the data storage table. Set status as running initialisation.
// If NV memory not ready, immediate return.
if (udb_nv_memory_read((uint8_t*) &data_storage_table, 0, sizeof(data_storage_table), &data_storage_init_read_callback) == false) return;
data_storage_status = DATA_STORAGE_STATUS_INIT;
break;
case DATA_STORAGE_CHECK_TABLE:
#if (MANUAL_ERASE_TABLE == 1)
if (!data_storage_format_table())
data_storage_status = DATA_STORAGE_STATUS_START;
#else
if (data_storage_check_table() == false)
{
// If table check fails then format the table
if (!data_storage_format_table())
data_storage_status = DATA_STORAGE_STATUS_START;
}
else
{
data_storage_status = DATA_STORAGE_STATUS_WAITING;
}
#endif
break;
case DATA_STORAGE_WRITE:
// Write data to nv memory
// If NV memory not ready, immediate return.
switch (data_storage_type)
{
case DATA_STORAGE_CHECKSUM_STRUCT:
if (udb_nv_memory_write(pdata_storage_data,
data_storage_table.table[data_storage_handle].data_address + sizeof(DATA_STORAGE_HEADER),
data_storage_data_size,
&storage_write_callback) == false)
{
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
return;
}
break;
case DATA_STORAGE_SELF_MANAGED:
if (udb_nv_memory_write(pdata_storage_data,
data_storage_table.table[data_storage_handle].data_address,
data_storage_size,
&storage_write_callback) == false)
{
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
return;
}
break;
}
data_storage_status = DATA_STORAGE_WRITING_DATA;
break;
case DATA_STORAGE_WRITING_DATA_COMPLETE:
{
switch (data_storage_type)
{
case DATA_STORAGE_CHECKSUM_STRUCT:
{
data_storage_header.data_checksum = crc_calculate((uint8_t*)pdata_storage_data, data_storage_data_size);
data_storage_header.data_handle = data_storage_handle;
data_storage_header.data_version = 0;
memcpy(data_storage_header.data_preamble, data_storage_preamble, sizeof(data_storage_preamble));
if (udb_nv_memory_write((uint8_t*)&data_storage_header,
data_storage_table.table[data_storage_handle].data_address,
sizeof(DATA_STORAGE_HEADER),
&storage_write_header_callback) == false)
{
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
}
else
data_storage_status = DATA_STORAGE_WRITING_HEADER;
}
break;
case DATA_STORAGE_SELF_MANAGED:
if (data_storage_user_callback != NULL)
data_storage_user_callback(true);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
break;
}
}
break;
case DATA_STORAGE_READ:
data_storage_type = data_storage_table.table[data_storage_handle].data_type;
switch (data_storage_type)
{
case DATA_STORAGE_CHECKSUM_STRUCT:
if (udb_nv_memory_read((uint8_t*) &data_storage_header,
data_storage_table.table[data_storage_handle].data_address,
sizeof(DATA_STORAGE_HEADER),
&storage_read_header_callback) == false)
{
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
}
else
data_storage_status = DATA_STORAGE_READING_HEADER;
break;
case DATA_STORAGE_SELF_MANAGED:
if (udb_nv_memory_read(pdata_storage_data,
data_storage_table.table[data_storage_handle].data_address,
data_storage_size,
&storage_read_data_callback) == false)
{
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
}
else
data_storage_status = DATA_STORAGE_READING_DATA;
break;
}
break;
case DATA_STORAGE_READ_HEADER_COMPLETE:
{
if (udb_nv_memory_read(pdata_storage_data,
data_storage_table.table[data_storage_handle].data_address + sizeof(DATA_STORAGE_HEADER),
data_storage_data_size,
&storage_read_data_callback) == false)
{
if (data_storage_user_callback != NULL) data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
}
else
data_storage_status = DATA_STORAGE_READING_DATA;
}
break;
case DATA_STORAGE_READ_DATA_COMPLETE:
{
boolean success = true;
if (data_storage_type == DATA_STORAGE_CHECKSUM_STRUCT)
{
// If handle is incorrect then fail
if (data_storage_header.data_handle != data_storage_handle) success = false;
// If preamble is incorrect then fail
if (memcmp(data_storage_header.data_preamble, data_storage_preamble, 4) != 0) success = false;
// If checksum is incorrect then fail.
if (data_storage_header.data_checksum != crc_calculate((uint8_t*) pdata_storage_data, data_storage_data_size))
success = false;
}
// Status to waiting and callback the user with result
data_storage_status = DATA_STORAGE_STATUS_WAITING;
if (data_storage_user_callback != NULL) data_storage_user_callback(success);
}
break;
case DATA_STORAGE_AREA_CREATE:
if ((data_storage_table.table[data_storage_handle].data_address = data_storage_find_hole(data_storage_size)) == 0)
{
// Failed to find space for new data area
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
break;
}
data_storage_table.table[data_storage_handle].data_size = data_storage_size;
data_storage_table.table[data_storage_handle].data_type = data_storage_type;
data_storage_status = DATA_STORAGE_AREA_CREATING;
// Calculate the checksum
data_storage_table.table_checksum = crc_calculate((uint8_t*) data_storage_table.table, sizeof(*data_storage_table.table));
// Store the table. If storage area create fails, put the services in wait.
if (udb_nv_memory_write((uint8_t*) &data_storage_table, 0, sizeof(data_storage_table), &data_storage_write_table_callback) == false)
{
data_storage_table.table[data_storage_handle].data_size = 0;
data_storage_table.table[data_storage_handle].data_type = 0;
data_storage_table.table[data_storage_handle].data_address = 0;
if (data_storage_user_callback != NULL)
data_storage_user_callback(false);
data_storage_status = DATA_STORAGE_STATUS_WAITING;
return;
}
else
data_storage_status = DATA_STORAGE_AREA_CREATING;
break;
case DATA_STORAGE_AREA_CLEAR:
storage_clear_specific_area();
break;
}
}
/*
setup mixer on PX4 so that if FMU dies the pilot gets manual control
*/
bool Plane::setup_failsafe_mixing(void)
{
const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX";
bool ret = false;
char *buf = NULL;
const uint16_t buf_size = 2048;
uint16_t fileSize, new_crc;
int px4io_fd = -1;
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
buf = (char *)malloc(buf_size);
if (buf == NULL) {
goto failed;
}
fileSize = create_mixer(buf, buf_size, mixer_filename);
if (!fileSize) {
hal.console->printf("Unable to create mixer\n");
goto failed;
}
new_crc = crc_calculate((uint8_t *)buf, fileSize);
if ((int32_t)new_crc == last_mixer_crc) {
free(buf);
return true;
} else {
last_mixer_crc = new_crc;
}
px4io_fd = open("/dev/px4io", 0);
if (px4io_fd == -1) {
// px4io isn't started, no point in setting up a mixer
goto failed;
}
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
// make sure the throttle has a non-zero failsafe value before we
// disable safety. This prevents sending zero PWM during switch over
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
}
// we need to force safety on to allow us to load a mixer. We call
// it twice as there have been reports that this call can fail
// with a small probability
hal.rcout->force_safety_on();
hal.rcout->force_safety_no_wait();
/* reset any existing mixer in px4io. This shouldn't be needed,
* but is good practice */
if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) {
hal.console->printf("Unable to reset mixer\n");
goto failed;
}
/* pass the buffer to the device */
if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) {
hal.console->printf("Unable to send mixer to IO\n");
goto failed;
}
// setup RC config for each channel based on user specified
// mix/max/trim. We only do the first 8 channels due to
// a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS
for (uint8_t i=0; i<8; i++) {
RC_Channel *ch = RC_Channel::rc_channel(i);
if (ch == NULL) {
continue;
}
struct pwm_output_rc_config config;
/*
we use a min/max of 900/2100 to allow for pass-thru of
larger values than the RC min/max range. This mimics the APM
behaviour of pass-thru in manual, which allows for dual-rate
transmitter setups in manual mode to go beyond the ranges
used in stabilised modes
*/
config.channel = i;
config.rc_min = 900;
config.rc_max = 2100;
if (rcmap.throttle()-1 == i) {
// throttle uses a trim of 1500, so we don't get division
// by small numbers near RC3_MIN
config.rc_trim = 1500;
} else {
config.rc_trim = constrain_int16(ch->get_radio_trim(), config.rc_min+1, config.rc_max-1);
}
config.rc_dz = 0; // zero for the purposes of manual takeover
// we set reverse as false, as users of ArduPilot will have
// input reversed on transmitter, so from the point of view of
// the mixer the input is never reversed. The one exception is
// the 2nd channel, which is reversed inside the PX4IO code,
// so needs to be unreversed here to give sane behaviour.
if (i == 1) {
config.rc_reverse = true;
} else {
config.rc_reverse = false;
}
if (i+1 == g.override_channel.get()) {
/*
This is an OVERRIDE_CHAN channel. We want IO to trigger
override with a channel input of over 1750. The px4io
code is setup for triggering below 80% of the range below
trim. To map this to values above 1750 we need to reverse
the direction and set the rc range for this channel to 1000
to 1813 (1812.5 = 1500 + 250/0.8)
*/
config.rc_assignment = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
config.rc_reverse = true;
config.rc_max = 1813; // round 1812.5 up to grant > 1750
config.rc_min = 1000;
config.rc_trim = 1500;
} else {
config.rc_assignment = i;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) {
hal.console->printf("SET_RC_CONFIG failed\n");
goto failed;
}
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
pwm_values.values[i] = 900;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) {
hal.console->printf("SET_MIN_PWM failed\n");
goto failed;
}
for (uint8_t i = 0; i < pwm_values.channel_count; i++) {
pwm_values.values[i] = 2100;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) {
hal.console->printf("SET_MAX_PWM failed\n");
goto failed;
}
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0) != 0) {
hal.console->printf("SET_OVERRIDE_OK failed\n");
goto failed;
}
// setup for immediate manual control if FMU dies
if (ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1) != 0) {
hal.console->printf("SET_OVERRIDE_IMMEDIATE failed\n");
goto failed;
}
ret = true;
failed:
if (buf != NULL) {
free(buf);
}
if (px4io_fd != -1) {
close(px4io_fd);
}
// restore safety state if it was previously armed
if (old_state == AP_HAL::Util::SAFETY_ARMED) {
hal.rcout->force_safety_off();
}
if (!ret) {
// clear out the mixer CRC so that we will attempt to send it again
last_mixer_crc = -1;
}
return ret;
}
static void rx_callback(phy_rx_data_t* res)
{
//log_packet(res->data);
if (res == NULL)
{
scan_timeout();
return;
}
// Data Link Filtering
// Subnet Matching do not parse it yet
if (dll_state == DllStateScanBackgroundFrame)
{
uint16_t crc = crc_calculate(res->data, 4);
if (memcmp((uint8_t*) &(res->data[4]), (uint8_t*) &crc, 2) != 0)
{
#ifdef LOG_DLL_ENABLED
log_print_stack_string(LOG_DLL, "DLL CRC ERROR");
#endif
scan_next(NULL); // how to reïnitiate scan on CRC Error, PHY should stay in RX
return;
}
if (!check_subnet(0xFF, res->data[0])) // TODO: get device_subnet from datastore
{
#ifdef LOG_DLL_ENABLED
log_print_stack_string(LOG_DLL, "DLL Subnet mismatch");
#endif
scan_next(NULL); // how to reïnitiate scan on subnet mismatch, PHY should stay in RX
return;
}
} else if (dll_state == DllStateScanForegroundFrame)
{
uint16_t crc = crc_calculate(res->data, res->length - 2);
if (memcmp((uint8_t*) &(res->data[res->length - 2]), (uint8_t*) &crc, 2) != 0)
{
#ifdef LOG_DLL_ENABLED
log_print_stack_string(LOG_DLL, "DLL CRC ERROR");
#endif
scan_next(NULL); // how to reïnitiate scan on CRC Error, PHY should stay in RX
return;
}
if (!check_subnet(0xFF, res->data[2])) // TODO: get device_subnet from datastore
{
#ifdef LOG_DLL_ENABLED
log_print_stack_string(LOG_DLL, "DLL Subnet mismatch");
#endif
scan_next(NULL); // how to reïnitiate scan on subnet mismatch, PHY should stay in RX
return;
}
} else
{
#ifdef LOG_DLL_ENABLED
log_print_stack_string(LOG_DLL, "DLL You fool, you can't be here");
#endif
}
// Optional Link Quality Assessment
// parse packet
dll_res.rssi = res->rssi;
dll_res.lqi = res->lqi;
dll_res.spectrum_id = current_css->values[current_scan_id].spectrum_id;
if (dll_state == DllStateScanBackgroundFrame)
{
dll_background_frame_t* frame = (dll_background_frame_t*)frame_data;
frame->subnet = res->data[0];
memcpy(frame->payload, res->data+1, 4);
dll_res.frame_type = FrameTypeBackgroundFrame;
dll_res.frame = frame;
}
else
{
dll_foreground_frame_t* frame = (dll_foreground_frame_t*)frame_data;
frame->length = res->data[0];
frame->frame_header.tx_eirp = res->data[1] * 0.5 - 40;
frame->frame_header.subnet = res->data[2];
frame->frame_header.frame_ctl = res->data[3];
uint8_t* data_pointer = res->data + 4;
if (frame->frame_header.frame_ctl & FRAME_CTL_LISTEN) // Listen
timeout_listen = 10;
else
timeout_listen = 0;
if (frame->frame_header.frame_ctl & FRAME_CTL_DLLS) // DLLS present
{
// TODO parse DLLS Header
frame->dlls_header = NULL;
} else {
frame->dlls_header = NULL;
}
if (frame->frame_header.frame_ctl & 0x20) // Enable Addressing
{
// Address Control Header
dll_foreground_frame_address_ctl_t address_ctl;// = (dll_foreground_frame_address_ctl_t*) data_pointer;
frame->address_ctl = &address_ctl;
frame->address_ctl->dialogId = *data_pointer;
data_pointer++;
frame->address_ctl->flags = *data_pointer;
data_pointer++;
//data_pointer += sizeof(uint8_t*);
uint8_t addressing = (frame->address_ctl->flags & 0xC0) >> 6;
uint8_t vid = (frame->address_ctl->flags & 0x20) >> 5;
uint8_t nls = (frame->address_ctl->flags & 0x10) >> 4;
// TODO parse Source ID Header
frame->address_ctl->source_id = data_pointer;
if (vid)
{
data_pointer += 2;
}
else
{
data_pointer += 8;
}
if (addressing == 0 && nls == 0)
{
uint8_t id_target[8];
if (vid)
{
memcpy(data_pointer, &id_target, 2);
data_pointer += 2;
}
else
{
memcpy(data_pointer, &id_target, 8);
data_pointer += 8;
}
frame->address_ctl->target_id = (uint8_t*) &id_target;
} else {
frame->address_ctl->target_id = NULL;
}
} else {
