add oda af and simplify get_next_af

src/ascii_cmd.c

@@ -163,6 +163,42 @@ static void handle_af(char *arg, RDSModulator* mod, char* output) {
 	strcpy(output, "+\0");
 }
 
+static void handle_afo(char *arg, RDSModulator* mod, char* output) {
+	if (arg[0] == 'A' || arg[0] == 'B') {
+		strcpy(output, "-\0");
+		return;
+	}
+	if(arg[0] == '\0') {
+		memset(&(mod->enc->data[mod->enc->program].af_oda), 0, sizeof(mod->enc->data[mod->enc->program].af_oda));
+		return;
+	}
+
+	memset(&(mod->enc->data[mod->enc->program].af_oda), 0, sizeof(mod->enc->data[mod->enc->program].af_oda));
+	uint8_t arg_count;
+	RDSAFsODA new_af_oda;
+	float af[MAX_AFS], *af_iter;
+
+	arg_count = sscanf((char *)arg,
+		"%f,%f,%f,%f,%f,"
+		"%f,%f,%f,%f,%f,"
+		"%f,%f,%f,%f,%f,"
+		"%f,%f,%f,%f,%f",
+		&af[0],  &af[1],  &af[2],  &af[3],  &af[4],
+		&af[5],  &af[6],  &af[7],  &af[8],  &af[9],
+		&af[10], &af[11], &af[12], &af[13], &af[14],
+		&af[15], &af[16], &af[17], &af[18], &af[19]);
+
+	af_iter = af;
+	memset(&new_af_oda, 0, sizeof(RDSAFsODA));
+
+	while (arg_count-- != 0) {
+		add_rds_af_oda(&new_af_oda, *af_iter++);
+	}
+
+	memcpy(&(mod->enc->data[mod->enc->program].af_oda), &new_af_oda, sizeof(mod->enc->data[mod->enc->program].af_oda));
+	strcpy(output, "+\0");
+}
+
 static void handle_g(char *arg, RDSModulator* mod, char* output) {
 	uint16_t blocks[4];
 	int count = sscanf((char *)arg, "%4hx%4hx%4hx%4hx", &blocks[0], &blocks[1], &blocks[2], &blocks[3]);
@@ -450,6 +486,7 @@ static const command_handler_t commands_eq4[] = {
 	{"RTP", handle_rtp, 3},
 	{"LPS", handle_lps, 3},
 	{"ERT", handle_ert, 3},
+	{"AFO", handle_afo, 3}
 };
 
 static const command_handler_t commands_eq5[] = {

src/lib.c

@@ -81,6 +81,41 @@ void add_checkwords(RDSGroup *group, uint8_t *bits)
 	}
 }
 
+uint8_t add_rds_af_oda(RDSAFsODA *af_list, float freq) {
+	uint16_t af;
+
+	uint8_t entries_reqd = 1;
+	if (freq < 64.1f || freq > 107.9f) entries_reqd = 2;
+
+	if (af_list->num_afs + entries_reqd > MAX_AFS) return 1;
+
+	if(freq >= 64.1f && freq <= 88.0f) {
+		af = (uint16_t)(freq * 10.0f) - 384;
+		af_list->afs[af_list->num_entries] = af;
+		af_list->num_entries += 1;
+	} else if (freq >= 87.6f && freq <= 107.9f) {
+		af = (uint16_t)(freq * 10.0f) - 875;
+		af_list->afs[af_list->num_entries] = af;
+		af_list->num_entries += 1;
+	} else if (freq >= 153.0f && freq <= 279.0f) {
+		af = (uint16_t)(freq - 153.0f) / 9 + 1;
+		af_list->afs[af_list->num_entries + 0] = AF_CODE_LFMF_FOLLOWS;
+		af_list->afs[af_list->num_entries + 1] = af;
+		af_list->num_entries += 2;
+	} else if (freq >= 531.0f && freq <= 1602.0f) {
+		af = (uint16_t)(freq - 531.0f) / 9 + 16;
+		af_list->afs[af_list->num_entries + 0] = AF_CODE_LFMF_FOLLOWS;
+		af_list->afs[af_list->num_entries + 1] = af;
+		af_list->num_entries += 2;
+	} else {
+		return 1;
+	}
+
+	af_list->num_afs++;
+
+	return 0;
+}
+
 uint8_t add_rds_af(RDSAFs *af_list, float freq) {
 	uint16_t af;
 

src/lib.h

@@ -7,5 +7,6 @@ extern uint16_t crc16_ccitt(char *data, uint16_t len);
 uint16_t get_block_grom_group(RDSGroup *group, uint8_t block);
 
 extern void add_checkwords(RDSGroup *group, uint8_t *bits);
+extern uint8_t add_rds_af_oda(RDSAFsODA *af_list, float freq);
 extern uint8_t add_rds_af(RDSAFs *af_list, float freq);
 extern char *convert_to_rds_charset(char *str);

src/rds.c

@@ -98,20 +98,20 @@ static uint16_t get_next_af(RDSEncoder* enc) {
 	uint16_t out;
 
 	if (enc->data[enc->program].af.num_afs) {
-		if (enc->state[enc->program].af_state == 0) {
-			out = (AF_CODE_NUM_AFS_BASE + enc->data[enc->program].af.num_afs) << 8;
-			out |= enc->data[enc->program].af.afs[0];
-			enc->state[enc->program].af_state += 1;
+		uint16_t* afs = enc->data[enc->program].af.afs;
+		uint16_t num_afs = enc->data[enc->program].af.num_afs;
+		uint16_t* af_state = &enc->state[enc->program].af_state;
+
+		if (*af_state < num_afs) {
+			out = afs[*af_state];
+			(*af_state)++;
 		} else {
-			out = enc->data[enc->program].af.afs[enc->state[enc->program].af_state] << 8;
-			if (enc->data[enc->program].af.afs[enc->state[enc->program].af_state + 1]) {
-				out |= enc->data[enc->program].af.afs[enc->state[enc->program].af_state + 1];
-			} else {
-				out |= AF_CODE_FILLER;
-				enc->state[enc->program].af_state += 2;
-			}
+			out = AF_CODE_FILLER;
+		}
+
+		if (*af_state >= enc->data[enc->program].af.num_entries) {
+			*af_state = 0;
 		}
-		if (enc->state[enc->program].af_state >= enc->data[enc->program].af.num_entries) enc->state[enc->program].af_state = 0;
 	} else {
 		out = AF_CODE_NUM_AFS_BASE << 8 | AF_CODE_FILLER;
 	}
@@ -119,6 +119,25 @@ static uint16_t get_next_af(RDSEncoder* enc) {
 	return out;
 }
 
+static void get_next_af_oda(RDSEncoder* enc, uint16_t* af_group) {
+	uint16_t* afs = enc->data[enc->program].af_oda.afs;
+	uint16_t num_afs = enc->data[enc->program].af_oda.num_afs;
+	uint16_t* af_state = &enc->state[enc->program].af_oda_state;
+
+	for (int i = 0; i < 4; ++i) {
+		if (*af_state < num_afs) {
+			af_group[i] = afs[*af_state];
+			(*af_state)++;
+		} else {
+			af_group[i] = AF_CODE_FILLER;
+		}
+	}
+
+	if (*af_state >= enc->data[enc->program].af_oda.num_entries) {
+		*af_state = 0;
+	}
+}
+
 static uint16_t get_next_af_eon(RDSEncoder* enc, uint8_t eon_index) {
 	uint16_t out;
 
@@ -257,6 +276,32 @@ static void get_rds_ert_oda_group(RDSEncoder* enc, RDSGroup *group) {
 	group->d = ODA_AID_ERT;
 }
 
+static void get_oda_af_oda_group(RDSEncoder* enc, RDSGroup *group) {
+	(void)enc;
+	group->b |= 3 << 12;
+
+	group->b |= 9 << 1;
+	group->c = ODA_AID_ODAAF;
+}
+
+static void get_oda_af_group(RDSEncoder* enc, RDSGroup *group) {
+	uint16_t af[4];
+	get_next_af_oda(enc, &af);
+
+	group->b |= 9 << 12;
+	for (int i = 0; i < 4; i++) {
+		group->b |= ((af[i] >> 15) & 1) << i;
+	}
+
+	group->c = af[0] & 0xFFFF;
+	group->c <<= 8;
+	group->c |= af[1] & 0xFFFF;
+
+	group->d = af[2] & 0xFFFF;
+	group->d <<= 8;
+	group->d |= af[3] & 0xFFFF;
+}
+
 static void get_rds_ct_group(RDSEncoder* enc, RDSGroup *group) {
 	(void)enc;
 	struct tm *utc, *local_time;
@@ -533,6 +578,14 @@ static void get_rds_sequence_group(RDSEncoder* enc, RDSGroup *group, char* grp,
 		case 'T':
 			get_rds_fasttuning_group(enc, group);
 			goto group_coded;
+		case 'U':
+			if(enc->state[enc->program].af_oda == 0) {
+				get_oda_af_group(enc, group);
+			} else {
+				get_oda_af_oda_group(enc, group);
+			}
+			enc->state[enc->program].af_oda ^= 1;
+			goto group_coded;
 	}
 group_coded:
 	return;
@@ -560,6 +613,7 @@ static uint8_t check_rds_good_group(RDSEncoder* enc, char* grp, uint8_t stream)
 	if(*grp == 'S' && enc->data[enc->program].ert[0] != '\0') good_group = 1;
 	if(*grp == 'F' && enc->data[enc->program].lps[0] != '\0') good_group = 1;
 	if(*grp == 'T') good_group = 1;
+	if(*grp == 'U' && enc->data[enc->program].af_oda.num_afs) good_group = 1;
 	return good_group;
 }
 

src/rds.h

@@ -35,7 +35,13 @@
 #define	ODA_AID_RTPLUS	0x4bd7
 #define	ODA_AID_ERT		0x6552
 #define	ODA_AID_ERTPLUS	0x4BD8
+#define ODA_AID_ODAAF	0x6365
 
+typedef struct {
+	uint8_t num_entries : 6;
+	uint8_t num_afs : 5;
+	uint16_t afs[MAX_AFS]; // 9 bit, there was no uint9_t
+} RDSAFsODA;
 typedef struct {
 	uint8_t num_entries : 6;
 	uint8_t num_afs : 5;
@@ -84,8 +90,8 @@ typedef struct {
 	uint8_t ptyn_enabled : 1;
 	char ptyn[PTYN_LENGTH];
 
-	uint8_t af_method : 1;
 	RDSAFs af;
+	RDSAFsODA af_oda;
 
 	uint8_t ct : 1;
 
@@ -142,6 +148,7 @@ typedef struct {
 	uint8_t rtp_oda : 1;
 	uint8_t ertp_oda : 1;
 	uint8_t ert_oda : 1;
+	uint8_t af_oda : 1;
 	uint8_t data_ecc : 1;
 	uint8_t grp_seq_idx[4];
 	uint8_t udg_idxs[2];
@@ -157,6 +164,7 @@ typedef struct {
 	RDSEONState eon_states[4];
 
 	uint8_t af_state : 6;
+	uint8_t af_oda_state : 6;
 
 	uint16_t last_stream0_group[3];
 } RDSState;