1717#define STEP_TOWARDS0 1
1818#define STEP_AWAYFROM0 0
1919
20+ static bool drive0_present ;
21+ static bool drive1_present ;
22+
2023static volatile uint32_t clock = 0 ; /* ms */
2124static volatile bool index_irq = false;
2225
@@ -42,6 +45,8 @@ static volatile bool dma_underrun = false;
4245#define DECLARE_REPLY_FRAME (STRUCT , TYPE ) \
4346 STRUCT r = {.f = { .type = TYPE, .size = sizeof(STRUCT) }}
4447
48+ static void stop_motor (void );
49+
4550static void system_timer_cb (void )
4651{
4752 CyGlobalIntDisable ;
@@ -104,7 +109,10 @@ static void print(const char* msg, ...)
104109static void set_drive_flags (struct set_drive_frame * flags )
105110{
106111 if (current_drive_flags .drive != flags -> drive )
112+ {
113+ stop_motor ();
107114 homed = false;
115+ }
108116
109117 current_drive_flags = * flags ;
110118 DRIVESELECT_REG_Write (flags -> drive ? 2 : 1 ); /* select drive 1 or 0 */
@@ -184,19 +192,19 @@ static void step(int dir)
184192 CyDelay (STEP_INTERVAL_TIME );
185193}
186194
187- static void home (void )
195+ /* returns true if it looks like a drive is attached */
196+ static bool home (void )
188197{
189198 for (int i = 0 ; i < 100 ; i ++ )
190199 {
191200 /* Don't keep stepping forever, because if a drive's
192201 * not connected bad things happen. */
193202 if (TRACK0_REG_Read ())
194- break ;
203+ return true ;
195204 step (STEP_TOWARDS0 );
196205 }
197206
198- /* Step to -1, which should be a nop, to reset the disk on disk change. */
199- step (STEP_TOWARDS0 );
207+ return false;
200208}
201209
202210static void seek_to (int track )
@@ -761,6 +769,10 @@ static void cmd_erase(struct erase_frame* f)
761769
762770static void cmd_set_drive (struct set_drive_frame * f )
763771{
772+ if (drive0_present && !drive1_present )
773+ f -> drive = 0 ;
774+ if (drive1_present && !drive0_present )
775+ f -> drive = 1 ;
764776 set_drive_flags (f );
765777
766778 DECLARE_REPLY_FRAME (struct any_frame , F_FRAME_SET_DRIVE_REPLY );
@@ -930,6 +942,18 @@ int main(void)
930942
931943 CyWdtStart (CYWDT_1024_TICKS , CYWDT_LPMODE_DISABLED );
932944
945+ current_drive_flags .drive = 0 ;
946+ start_motor ();
947+ drive0_present = home ();
948+ stop_motor ();
949+
950+ current_drive_flags .drive = 1 ;
951+ start_motor ();
952+ drive1_present = home ();
953+ stop_motor ();
954+
955+ print ("drive 0: %s drive 1: %s" , drive0_present ? "yes" : "no" , drive1_present ? "yes" : "no" );
956+
933957 /* UART_PutString("GO\r"); */
934958
935959 for (;;)
0 commit comments